Subversion Repositories gelsvn

Rev

Rev 290 | Rev 299 | Go to most recent revision | Details | Compare with Previous | Last modification | View Log | RSS feed

Rev Author Line No. Line
290 jrf 1 
#include <cfloat>
2 
#include <queue>
3 
#include <algorithm>
4 
#include <vector>
5 
#include "AABox.h"
6 
#include "OBox.h"
7 
#include "BoundingTree.h"
8 
 
9 
using namespace std;
10 
using namespace CGLA;
11 
 
12 
 
13 
namespace
14 
{
15 
	template <class _Tp>
16 
	inline const _Tp& my_min(const _Tp& __a, const _Tp& __b)
17 
	{
18 
		return __b < __a ? __b : __a;
19 
	}
20 
	template <class _Tp>
21 
	inline const _Tp& my_max(const _Tp& __a, const _Tp& __b)
22 
	{
23 
		return  __a < __b ? __b : __a;
24 
	}
25 
 
26 
}
27 
 
291 jrf 28 
namespace Geometry
29 
{
30 
 
290 jrf 31 
template<class BoxType>
32 
bool BoundingTree<BoxType>::intersect(const CGLA::Vec3f& p , const CGLA::Vec3f& dir,
33 
																float& tmin) const
34 
{
35 
	return root->intersect(p,dir,tmin);
36 
}
37 
 
38 
template<class BoxType>
39 
int BoundingTree<BoxType>::intersect_cnt(const CGLA::Vec3f& p,
40 
																	 const CGLA::Vec3f& dir) const
41 
{
42 
	return root->intersect_cnt(p,dir);
43 
}
44 
 
45 
template<class BoxType>
46 
void BoundingTree<BoxType>::build(std::vector<Triangle>& triangles)
47 
{
48 
	delete root;
49 
	root = Node::build(triangles);
50 
}
51 
 
52 
 
53 
template<class Node>
54 
class HE
55 
{
56 
	const Node* node;
57 
	float sq_dist_min;
58 
	float sq_dist_max;
59 
	float sgn;
60 
 
61 
public:
62 
 
63 
	HE() {}
64 
 
65 
	HE(const Vec3f& p, const Node*_node):
66 
		node(_node), sq_dist_min(FLT_MAX), sq_dist_max(FLT_MAX), sgn(0)
67 
 
68 
	{
69 
		node->sq_distance(p,sq_dist_min,sq_dist_max, sgn);
70 
	}
71 
 
72 
	float get_sq_dist_min() const {return sq_dist_min;}
73 
	float get_sq_dist_max() const {return sq_dist_max;}
74 
 
75 
	float get_dist() const {return sgn * sqrt(sq_dist_min);}
76 
 
77 
	const Node* get_node() const
78 
	{
79 
		return node;
80 
	}
81 
 
82 
	bool operator<(const HE<Node>& r)
83 
	{
84 
		return r.sq_dist_min< sq_dist_min;
85 
	}
86 
 
87 
};
88 
 
89 
 
90 
 
91 
template<class BoxType>
92 
float BoundingTree<BoxType>::compute_signed_distance(const CGLA::Vec3f& p,
93 
																										 float minmax) const
94 
{
95 
	int N=100;
96 
	vector<HE<Node> > Q(N);
97 
	Q[0] = HE<Node>(p,root);
98 
 
99 
	HE<Node> *Q_beg = &Q[0];
100 
	int Q_end = 1;
101 
	int pushes = 1;
102 
	while(const IntNode* n = dynamic_cast<const IntNode*>(Q[0].get_node()))
103 
		{
104 
			float q0_max= Q[0].get_sq_dist_max();
105 
			float q0_min= Q[0].get_sq_dist_min();
106 
			pop_heap(Q_beg, Q_beg + Q_end);
107 
			--Q_end;
108 
 
109 
 
110 
			HE<Node> hl(p,n->get_left());
111 
			if(hl.get_sq_dist_min() < (minmax + DIST_THRESH))
112 
				{
113 
					if(hl.get_sq_dist_max() < minmax)
114 
							minmax = hl.get_sq_dist_max();
115 
 
116 
					Q[Q_end++] = hl;
117 
					push_heap(Q_beg, Q_beg + Q_end);
118 
					if(Q_end == N)
119 
						{
120 
							Q.resize(N=2*N);
121 
							Q_beg = &Q[0];
122 
						}
123 
					++pushes;
124 
				}
125 
 
126 
			HE<Node> hr(p,n->get_right());
127 
			if(hr.get_sq_dist_min() < (minmax + DIST_THRESH))
128 
				{
129 
					if(hr.get_sq_dist_max() < minmax)
130 
							minmax = hr.get_sq_dist_max();
131 
 
132 
					Q[Q_end++] = hr;
133 
					push_heap(Q_beg, Q_beg + Q_end);
134 
					if(Q_end == N)
135 
						{
136 
							Q.resize(N=2*N);
137 
							Q_beg = &Q[0];
138 
						}
139 
					++pushes;
140 
				}
141 
 
142 
//  			if((hr.get_sq_dist_min() > (q0_max + DIST_THRESH)) &&
143 
// 				 (hl.get_sq_dist_min() > (q0_max + DIST_THRESH)) )
144 
// 				{
145 
// 					cout.precision(4);
146 
// 					cout << q0_min << " " << q0_max << endl;
147 
// 					cout << hl.get_sq_dist_min() << endl;
148 
// 					cout << hr.get_sq_dist_min() << endl;
149 
// 					cout << typeid(*n).name() << endl;
150 
// 					if(const LeafNode* ll =dynamic_cast<const LeafNode*>(hl.get_node()))
151 
// 						{
152 
// 							ll->get_tri().print();
153 
// 							cout << sqr_length(p-ll->get_tri().get_v0()) << endl;
154 
// 							cout << sqr_length(p-ll->get_tri().get_v1()) << endl;
155 
// 							cout << sqr_length(p-ll->get_tri().get_v2()) << endl;
156 
// 							float d=FLT_MAX, s;
157 
// 							ll->get_tri().signed_distance(p,d,s);
158 
// 							cout << "Dist " << d << endl;
159 
// 						}
160 
// 					if(const LeafNode* lr =dynamic_cast<const LeafNode*>(hr.get_node()))
161 
// 						{
162 
// 							lr->get_tri().print();
163 
// 							cout << sqr_length(p-lr->get_tri().get_v0()) << endl;
164 
// 							cout << sqr_length(p-lr->get_tri().get_v1()) << endl;
165 
// 							cout << sqr_length(p-lr->get_tri().get_v2()) << endl;
166 
// 							float d=FLT_MAX, s;
167 
// 							lr->get_tri().signed_distance(p,d,s);
168 
// 							cout << "Dist " << d << endl;
169 
// 						}
170 
// 					cout << "P=" << p<< endl;
171 
// 				}
172 
 
173 
 			assert((hr.get_sq_dist_min() < (q0_max + DIST_THRESH)) ||
174 
 						 (hl.get_sq_dist_min() < (q0_max + DIST_THRESH)) );
175 
			assert(Q_end > 0);
176 
			assert(Q_end <=N);
177 
		}
178 
	return Q[0].get_dist();
179 
}
180 
 
181 
template BoundingTree<AABox>;
182 
template BoundingTree<OBox>;
291 jrf 183 
 
184 
}