WebSVN – gelsvn – Diff – /trunk/src/Geometry/BoundingTree.cpp


/* ----------------------------------------------------------------------- *
 * This file is part of GEL, http://www.imm.dtu.dk/GEL
 * Copyright (C) the authors and DTU Informatics
 * For license and list of authors, see ../../doc/intro.pdf
 * ----------------------------------------------------------------------- */
 
#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) const
	{
		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 class BoundingTree<AABox>;
template class BoundingTree<OBox>;
 
}
 

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

Subversion Repositories gelsvn

(root)/trunk/src/Geometry/BoundingTree.cpp – Rev 324 → 595