WebSVN – gelsvn – Diff – /branches/SPD/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>
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>;
 
}
 
Generated by GNU Enscript 1.6.6.
 
 
 

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

Subversion Repositories gelsvn

(root)/branches/SPD/src/Geometry/BoundingTree.cpp – Rev 290 → 291