WebSVN – gelsvn – Diff – /branches/ray-debug/src/Geometry/BoundingTree.cpp


#include <cfloat>
#include <queue>
#include <algorithm>
#include <vector>
#include "AABox.h"
#include "OBox.h"
#include "BoundingTree.h"
 
using namespace std;
using namespace CGLA;
 
 
namespace
{
	template <class _Tp>
	inline const _Tp& my_min(const _Tp& __a, const _Tp& __b)
	{
		return __b < __a ? __b : __a;
	}
	template <class _Tp>
	inline const _Tp& my_max(const _Tp& __a, const _Tp& __b) 
	{
		return  __a < __b ? __b : __a;
	}
 
}
 
namespace Geometry
{
 
template<class BoxType>
bool BoundingTree<BoxType>::intersect(const CGLA::Vec3f& p , const CGLA::Vec3f& dir,
																float& tmin) const 
{
	return root->intersect(p,dir,tmin);
}
 
template<class BoxType>
void BoundingTree<BoxType>::intersect(Ray& r) const 
{
		root->intersect(r);
}
 
template<class BoxType>
int BoundingTree<BoxType>::intersect_cnt(const CGLA::Vec3f& p, 
																	 const CGLA::Vec3f& dir) const
{
	return root->intersect_cnt(p,dir);
}
 
template<class BoxType>
void BoundingTree<BoxType>::build(std::vector<Triangle>& triangles)
{
	delete root;
	root = Node::build(triangles);
}
 
 
template<class Node>
class HE
{
	const Node* node;
	float sq_dist_min;
	float sq_dist_max;
	float sgn;
 
public:
 
	HE() {}
 
	HE(const Vec3f& p, const Node*_node): 
		node(_node), sq_dist_min(FLT_MAX), sq_dist_max(FLT_MAX), sgn(0)
																				
	{
		node->sq_distance(p,sq_dist_min,sq_dist_max, sgn);
	}
	
	float get_sq_dist_min() const {return sq_dist_min;}
	float get_sq_dist_max() const {return sq_dist_max;}
 
	float get_dist() const {return sgn * sqrt(sq_dist_min);}
 
	const Node* get_node() const 
	{
		return node;
	}
 
	bool operator<(const HE<Node>& r)
	{
		return r.sq_dist_min< sq_dist_min;
	}
 
};
 
 
 
template<class BoxType>
float BoundingTree<BoxType>::compute_signed_distance(const CGLA::Vec3f& p,
																										 float minmax) const
{
	int N=100;
	vector<HE<Node> > Q(N);
	Q[0] = HE<Node>(p,root);
	
	HE<Node> *Q_beg = &Q[0];
	int Q_end = 1;
	int pushes = 1;
	while(const IntNode* n = dynamic_cast<const IntNode*>(Q[0].get_node()))
		{
			float q0_max= Q[0].get_sq_dist_max();
			float q0_min= Q[0].get_sq_dist_min();
			pop_heap(Q_beg, Q_beg + Q_end);
			--Q_end;
			
 
			HE<Node> hl(p,n->get_left());
			if(hl.get_sq_dist_min() < (minmax + DIST_THRESH))
				{
					if(hl.get_sq_dist_max() < minmax)
							minmax = hl.get_sq_dist_max();
					
					Q[Q_end++] = hl;
					push_heap(Q_beg, Q_beg + Q_end);
					if(Q_end == N) 
						{
							Q.resize(N=2*N);
							Q_beg = &Q[0];
						}
					++pushes;
				}
 
			HE<Node> hr(p,n->get_right());
			if(hr.get_sq_dist_min() < (minmax + DIST_THRESH))
				{
					if(hr.get_sq_dist_max() < minmax)
							minmax = hr.get_sq_dist_max();
 
					Q[Q_end++] = hr;
					push_heap(Q_beg, Q_beg + Q_end);
					if(Q_end == N)
						{
							Q.resize(N=2*N);
							Q_beg = &Q[0];
						}
					++pushes;
				}
 
//  			if((hr.get_sq_dist_min() > (q0_max + DIST_THRESH)) &&
// 				 (hl.get_sq_dist_min() > (q0_max + DIST_THRESH)) )
// 				{
// 					cout.precision(4);
// 					cout << q0_min << " " << q0_max << endl;
// 					cout << hl.get_sq_dist_min() << endl;
// 					cout << hr.get_sq_dist_min() << endl;
// 					cout << typeid(*n).name() << endl;
// 					if(const LeafNode* ll =dynamic_cast<const LeafNode*>(hl.get_node()))
// 						{
// 							ll->get_tri().print();
// 							cout << sqr_length(p-ll->get_tri().get_v0()) << endl;
// 							cout << sqr_length(p-ll->get_tri().get_v1()) << endl;
// 							cout << sqr_length(p-ll->get_tri().get_v2()) << endl;
// 							float d=FLT_MAX, s;
// 							ll->get_tri().signed_distance(p,d,s);
// 							cout << "Dist " << d << endl;
// 						}
// 					if(const LeafNode* lr =dynamic_cast<const LeafNode*>(hr.get_node()))
// 						{
// 							lr->get_tri().print();
// 							cout << sqr_length(p-lr->get_tri().get_v0()) << endl;
// 							cout << sqr_length(p-lr->get_tri().get_v1()) << endl;
// 							cout << sqr_length(p-lr->get_tri().get_v2()) << endl;
// 							float d=FLT_MAX, s;
// 							lr->get_tri().signed_distance(p,d,s);
// 							cout << "Dist " << d << endl;
// 						}
// 					cout << "P=" << p<< endl;
// 				}
 
 			assert((hr.get_sq_dist_min() < (q0_max + DIST_THRESH)) ||
 						 (hl.get_sq_dist_min() < (q0_max + DIST_THRESH)) );
			assert(Q_end > 0);
			assert(Q_end <=N);
		}
	return Q[0].get_dist();
}
 
template BoundingTree<AABox>;
template BoundingTree<OBox>;
 
 
}
 
 
 
 
 

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

Subversion Repositories gelsvn

(root)/branches/ray-debug/src/Geometry/BoundingTree.cpp – Rev 307 → 308