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

Subversion Repositories gelsvn

(root)/branches/cpp11-devel/src/Geometry/BoundingTree.cpp – Rev 595 → 630