WebSVN – gelsvn – Diff – /branches/cpp11-devel/src/Geometry/OBox.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 "../CGLA/statistics.h"
#include "../CGLA/eigensolution.h"
#include "../CGLA/Mat4x4f.h"
#include "AABox.h"
#include "OBox.h"
#include "Triangle.h"
 
using namespace std;
using namespace CGLA;
 
namespace
{
 
	Mat3x3f compute_rotation(const vector<Geometry::Triangle>& invec)
	{
		const int N_tri = invec.size();
 
		Mat3x3f C;
		float a_H = 0;
		Vec3f m_H(0);
		
		for(int i=0;i<N_tri;++i)
			{
				const Geometry::Triangle& tri = invec[i];
				
				float a_k = tri.area();
				a_H += a_k;
				
				Vec3f m_k = tri.centre();
				m_H += a_k * m_k;
				
				Vec3f p_k = tri.get_v0();
				Vec3f q_k = tri.get_v1();
				Vec3f r_k = tri.get_v2();
				
				Mat3x3f M,P,Q,R;
				outer_product(m_k,m_k, M);
				outer_product(p_k,p_k, P);
				outer_product(q_k,q_k, Q);
				outer_product(r_k,r_k, R);
				
				C += (a_k/12.0f) * (9*M+P+Q+R);
			}
		m_H /= a_H;
		C /= a_H;
		Mat3x3f M;
		outer_product(m_H, m_H, M);
		C -= M;
 
		Mat3x3f Q,L;
		const int n_eig = power_eigensolution(C,Q,L,2);
		
		Vec3f X = normalize(Q[0]);
		Vec3f Y = normalize(Q[1]);
		Vec3f Z;
		
		float xy_ortho = fabs(dot(X,Y));
		if(n_eig == 2 && xy_ortho < 0.3)
			{
				if(xy_ortho > CGLA::TINY)
					Y = normalize(Y-X*dot(X,Y));
				Z = normalize(cross(X,Y));
			}
		else if(n_eig==1)
			{
				Y = Vec3f(X[2],X[0],X[1]);
				Y = normalize(Y-X*dot(X,Y));
				Z = normalize(cross(X,Y));
			}
		else
			{
				X=Vec3f(1,0,0);
				Y=Vec3f(0,1,0);
				Z=Vec3f(0,0,1);
			}
		return Mat3x3f(X,Y,Z);
		
	}
 
 
 
}
 
namespace Geometry
{
 
bool OBox::intersect(const CGLA::Vec3f& p, const CGLA::Vec3f& d) const 
{
	Vec3f pr = R * p;
	Vec3f dr = R * d;
	return aabox.intersect(pr, dr);
}
 
 
OBox OBox::box_triangle(const Triangle& t)
{
	Vec3f e0 = t.get_v1()-t.get_v0();
	Vec3f e1 = t.get_v2()-t.get_v1();
	Vec3f e2 = t.get_v0()-t.get_v2();
 
	Vec3f X,Y,Z;
	if(sqr_length(e0) > sqr_length(e1))
		{
			if(sqr_length(e0) > sqr_length(e2))
				{
					X = normalize(e0);
					Y = normalize(e1 - X * dot(X, e1));
				}
			else
				{
					X = normalize(e2);
					Y = normalize(e0 - X * dot(X, e0));
				}
		}
	else
		{
			if(sqr_length(e1) > sqr_length(e2))
				{
					X = normalize(e1);
					Y = normalize(e2 - X * dot(X, e2));
				}
			else
				{
					X = normalize(e2);
					Y = normalize(e0 - X * dot(X, e0));
				}
		}
	Z = cross(X,Y);
	
	const Mat3x3f Rot(X,Y,Z);
 
	Vec3f p0 = Rot * t.get_v0();
	Vec3f p1 = Rot * t.get_v1();
	Vec3f p2 = Rot * t.get_v2();
	Vec3f pmin = v_min(p0, v_min(p1, p2));
	Vec3f pmax = v_max(p0, v_max(p1, p2));
	
	Vec3f centre_close = v_max(pmin, v_min(pmax, Rot * t.get_centre()));
	return OBox(Rot, AABox(pmin, pmax, centre_close));
}
 
 
OBox OBox::box_and_split(const std::vector<Triangle>& invec,
													 std::vector<Triangle>& lvec,
													 std::vector<Triangle>& rvec)
{
	// Obtain the rotation matrix for the OBB
	const Mat3x3f Rot = compute_rotation(invec);
	const int N_tri = invec.size();
	const int N_pts = 3*N_tri;
 
	// Compute the rotated set of points and the extents of the point aligned 
	// BBox.
	vector<Vec3f> pts(N_pts);
	Vec3f tri_pmin(FLT_MAX), tri_pmax(-FLT_MAX);
	for(int i=0;i<N_tri;++i)
		{
			const Triangle& tri = invec[i];
			
			int offs = 3*i;
			pts[offs  ] = Rot*tri.get_v0();
			pts[offs+1] = Rot*tri.get_v1();
			pts[offs+2] = Rot*tri.get_v2();
			
			for(int j=0;j<3;++j)
				{
					tri_pmin = v_min(pts[offs+j], tri_pmin);
					tri_pmax = v_max(pts[offs+j], tri_pmax);
				}
		}
 
	// Find the point closest to the centre.
	const Vec3f centre = tri_pmin + 0.5f*(tri_pmax-tri_pmin);
	Vec3f centre_close;
	float min_dist = FLT_MAX;
	for(int i=0;i<N_pts;++i)
		{
			Vec3f v = pts[i];
			float sl = sqr_length(centre-v);
			if(sl < min_dist)
				{
					min_dist = sl;
					centre_close = v;
				}
		}
 
	// Partition the triangles
	const float thresh = centre[0];
	for(int i=0;i<N_tri;++i)
		{
			Vec3f p = Rot * invec[i].get_centre();
			if( p[0] > thresh)
				rvec.push_back(invec[i]);
			else
				lvec.push_back(invec[i]);
		}
 
	// If all triangles landed in one box, split them naively.
	if(lvec.empty() || rvec.empty())
		{
			lvec.clear();
			lvec.insert(lvec.end(),
									invec.begin(),
									invec.begin()+N_tri/2);
			rvec.clear();
			rvec.insert(rvec.end(),
									invec.begin()+N_tri/2,
									invec.end());
		}
		
	return OBox(Rot, AABox(tri_pmin, tri_pmax, centre_close));
}
 
}
 

Rev 601	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 "../CGLA/statistics.h"`	8	`#include "../CGLA/statistics.h"`
9	`#include "../CGLA/eigensolution.h"`	9	`#include "../CGLA/eigensolution.h"`
10	`#include "../CGLA/Mat4x4f.h"`	10	`#include "../CGLA/Mat4x4f.h"`
11	`#include "AABox.h"`	11	`#include "AABox.h"`
12	`#include "OBox.h"`	12	`#include "OBox.h"`
13	`#include "Triangle.h"`	13	`#include "Triangle.h"`
14		14
15	`using namespace std;`	15	`using namespace std;`
16	`using namespace CGLA;`	16	`using namespace CGLA;`
17		17
18	`namespace`	18	`namespace`
19	`{`	19	`{`
20		20
21	`Mat3x3f compute_rotation(const vector<Geometry::Triangle>& invec)`	21	`Mat3x3f compute_rotation(const vector<Geometry::Triangle>& invec)`
22	`{`	22	`{`
23	`const int N_tri = invec.size();`	23	`const int N_tri = invec.size();`
24		24
25	`Mat3x3f C;`	25	`Mat3x3f C;`
26	`float a_H = 0;`	26	`float a_H = 0;`
27	`Vec3f m_H(0);`	27	`Vec3f m_H(0);`
28		28
29	`for(int i=0;i<N_tri;++i)`	29	`for(int i=0;i<N_tri;++i)`
30	`{`	30	`{`
31	`const Geometry::Triangle& tri = invec[i];`	31	`const Geometry::Triangle& tri = invec[i];`
32		32
33	`float a_k = tri.area();`	33	`float a_k = tri.area();`
34	`a_H += a_k;`	34	`a_H += a_k;`
35		35
36	`Vec3f m_k = tri.centre();`	36	`Vec3f m_k = tri.centre();`
37	`m_H += a_k * m_k;`	37	`m_H += a_k * m_k;`
38		38
39	`Vec3f p_k = tri.get_v0();`	39	`Vec3f p_k = tri.get_v0();`
40	`Vec3f q_k = tri.get_v1();`	40	`Vec3f q_k = tri.get_v1();`
41	`Vec3f r_k = tri.get_v2();`	41	`Vec3f r_k = tri.get_v2();`
42		42
43	`Mat3x3f M,P,Q,R;`	43	`Mat3x3f M,P,Q,R;`
44	`outer_product(m_k,m_k, M);`	44	`outer_product(m_k,m_k, M);`
45	`outer_product(p_k,p_k, P);`	45	`outer_product(p_k,p_k, P);`
46	`outer_product(q_k,q_k, Q);`	46	`outer_product(q_k,q_k, Q);`
47	`outer_product(r_k,r_k, R);`	47	`outer_product(r_k,r_k, R);`
48		48
49	`C += (a_k/12.0f) * (9*M+P+Q+R);`	49	`C += (a_k/12.0f) * (9*M+P+Q+R);`
50	`}`	50	`}`
51	`m_H /= a_H;`	51	`m_H /= a_H;`
52	`C /= a_H;`	52	`C /= a_H;`
53	`Mat3x3f M;`	53	`Mat3x3f M;`
54	`outer_product(m_H, m_H, M);`	54	`outer_product(m_H, m_H, M);`
55	`C -= M;`	55	`C -= M;`
56		56
57	`Mat3x3f Q,L;`	57	`Mat3x3f Q,L;`
58	`const int n_eig = power_eigensolution(C,Q,L,2);`	58	`const int n_eig = power_eigensolution(C,Q,L,2);`
59		59
60	`Vec3f X = normalize(Q[0]);`	60	`Vec3f X = normalize(Q[0]);`
61	`Vec3f Y = normalize(Q[1]);`	61	`Vec3f Y = normalize(Q[1]);`
62	`Vec3f Z;`	62	`Vec3f Z;`
63		63
64	`float xy_ortho = fabs(dot(X,Y));`	64	`float xy_ortho = fabs(dot(X,Y));`
65	`if(n_eig == 2 && xy_ortho < 0.3)`	65	`if(n_eig == 2 && xy_ortho < 0.3)`
66	`{`	66	`{`
67	`if(xy_ortho > CGLA::TINY)`	67	`if(xy_ortho > CGLA::TINY)`
68	`Y = normalize(Y-X*dot(X,Y));`	68	`Y = normalize(Y-X*dot(X,Y));`
69	`Z = normalize(cross(X,Y));`	69	`Z = normalize(cross(X,Y));`
70	`}`	70	`}`
71	`else if(n_eig==1)`	71	`else if(n_eig==1)`
72	`{`	72	`{`
73	`Y = Vec3f(X[2],X[0],X[1]);`	73	`Y = Vec3f(X[2],X[0],X[1]);`
74	`Y = normalize(Y-X*dot(X,Y));`	74	`Y = normalize(Y-X*dot(X,Y));`
75	`Z = normalize(cross(X,Y));`	75	`Z = normalize(cross(X,Y));`
76	`}`	76	`}`
77	`else`	77	`else`
78	`{`	78	`{`
79	`X=Vec3f(1,0,0);`	79	`X=Vec3f(1,0,0);`
80	`Y=Vec3f(0,1,0);`	80	`Y=Vec3f(0,1,0);`
81	`Z=Vec3f(0,0,1);`	81	`Z=Vec3f(0,0,1);`
82	`}`	82	`}`
83	`return Mat3x3f(X,Y,Z);`	83	`return Mat3x3f(X,Y,Z);`
84		84
85	`}`	85	`}`
86		86
87		87
88		88
89	`}`	89	`}`
90		90
91	`namespace Geometry`	91	`namespace Geometry`
92	`{`	92	`{`
93		93
94	`bool OBox::intersect(const CGLA::Vec3f& p, const CGLA::Vec3f& d) const`	94	`bool OBox::intersect(const CGLA::Vec3f& p, const CGLA::Vec3f& d) const`
95	`{`	95	`{`
96	`Vec3f pr = R * p;`	96	`Vec3f pr = R * p;`
97	`Vec3f dr = R * d;`	97	`Vec3f dr = R * d;`
98	`return aabox.intersect(pr, dr);`	98	`return aabox.intersect(pr, dr);`
99	`}`	99	`}`
100		100
101		101
102	`OBox OBox::box_triangle(const Triangle& t)`	102	`OBox OBox::box_triangle(const Triangle& t)`
103	`{`	103	`{`
104	`Vec3f e0 = t.get_v1()-t.get_v0();`	104	`Vec3f e0 = t.get_v1()-t.get_v0();`
105	`Vec3f e1 = t.get_v2()-t.get_v1();`	105	`Vec3f e1 = t.get_v2()-t.get_v1();`
106	`Vec3f e2 = t.get_v0()-t.get_v2();`	106	`Vec3f e2 = t.get_v0()-t.get_v2();`
107		107
108	`Vec3f X,Y,Z;`	108	`Vec3f X,Y,Z;`
109	`if(sqr_length(e0) > sqr_length(e1))`	109	`if(sqr_length(e0) > sqr_length(e1))`
110	`{`	110	`{`
111	`if(sqr_length(e0) > sqr_length(e2))`	111	`if(sqr_length(e0) > sqr_length(e2))`
112	`{`	112	`{`
113	`X = normalize(e0);`	113	`X = normalize(e0);`
114	`Y = normalize(e1 - X * dot(X, e1));`	114	`Y = normalize(e1 - X * dot(X, e1));`
115	`}`	115	`}`
116	`else`	116	`else`
117	`{`	117	`{`
118	`X = normalize(e2);`	118	`X = normalize(e2);`
119	`Y = normalize(e0 - X * dot(X, e0));`	119	`Y = normalize(e0 - X * dot(X, e0));`
120	`}`	120	`}`
121	`}`	121	`}`
122	`else`	122	`else`
123	`{`	123	`{`
124	`if(sqr_length(e1) > sqr_length(e2))`	124	`if(sqr_length(e1) > sqr_length(e2))`
125	`{`	125	`{`
126	`X = normalize(e1);`	126	`X = normalize(e1);`
127	`Y = normalize(e2 - X * dot(X, e2));`	127	`Y = normalize(e2 - X * dot(X, e2));`
128	`}`	128	`}`
129	`else`	129	`else`
130	`{`	130	`{`
131	`X = normalize(e2);`	131	`X = normalize(e2);`
132	`Y = normalize(e0 - X * dot(X, e0));`	132	`Y = normalize(e0 - X * dot(X, e0));`
133	`}`	133	`}`
134	`}`	134	`}`
135	`Z = cross(X,Y);`	135	`Z = cross(X,Y);`
136		136
137	`const Mat3x3f Rot(X,Y,Z);`	137	`const Mat3x3f Rot(X,Y,Z);`
138		138
139	`Vec3f p0 = Rot * t.get_v0();`	139	`Vec3f p0 = Rot * t.get_v0();`
140	`Vec3f p1 = Rot * t.get_v1();`	140	`Vec3f p1 = Rot * t.get_v1();`
141	`Vec3f p2 = Rot * t.get_v2();`	141	`Vec3f p2 = Rot * t.get_v2();`
142	`Vec3f pmin = v_min(p0, v_min(p1, p2));`	142	`Vec3f pmin = v_min(p0, v_min(p1, p2));`
143	`Vec3f pmax = v_max(p0, v_max(p1, p2));`	143	`Vec3f pmax = v_max(p0, v_max(p1, p2));`
144		144
145	`Vec3f centre_close = v_max(pmin, v_min(pmax, Rot * t.get_centre()));`	145	`Vec3f centre_close = v_max(pmin, v_min(pmax, Rot * t.get_centre()));`
146	`return OBox(Rot, AABox(pmin, pmax, centre_close));`	146	`return OBox(Rot, AABox(pmin, pmax, centre_close));`
147	`}`	147	`}`
148		148
149		149
150	`OBox OBox::box_and_split(const std::vector<Triangle>& invec,`	150	`OBox OBox::box_and_split(const std::vector<Triangle>& invec,`
151	`std::vector<Triangle>& lvec,`	151	`std::vector<Triangle>& lvec,`
152	`std::vector<Triangle>& rvec)`	152	`std::vector<Triangle>& rvec)`
153	`{`	153	`{`
154	`// Obtain the rotation matrix for the OBB`	154	`// Obtain the rotation matrix for the OBB`
155	`const Mat3x3f Rot = compute_rotation(invec);`	155	`const Mat3x3f Rot = compute_rotation(invec);`
156	`const int N_tri = invec.size();`	156	`const int N_tri = invec.size();`
157	`const int N_pts = 3*N_tri;`	157	`const int N_pts = 3*N_tri;`
158		158
159	`// Compute the rotated set of points and the extents of the point aligned`	159	`// Compute the rotated set of points and the extents of the point aligned`
160	`// BBox.`	160	`// BBox.`
161	`vector<Vec3f> pts(N_pts);`	161	`vector<Vec3f> pts(N_pts);`
162	`Vec3f tri_pmin(FLT_MAX), tri_pmax(-FLT_MAX);`	162	`Vec3f tri_pmin(FLT_MAX), tri_pmax(-FLT_MAX);`
163	`for(int i=0;i<N_tri;++i)`	163	`for(int i=0;i<N_tri;++i)`
164	`{`	164	`{`
165	`const Triangle& tri = invec[i];`	165	`const Triangle& tri = invec[i];`
166		166
167	`int offs = 3*i;`	167	`int offs = 3*i;`
168	`pts[offs ] = Rot*tri.get_v0();`	168	`pts[offs ] = Rot*tri.get_v0();`
169	`pts[offs+1] = Rot*tri.get_v1();`	169	`pts[offs+1] = Rot*tri.get_v1();`
170	`pts[offs+2] = Rot*tri.get_v2();`	170	`pts[offs+2] = Rot*tri.get_v2();`
171		171
172	`for(int j=0;j<3;++j)`	172	`for(int j=0;j<3;++j)`
173	`{`	173	`{`
174	`tri_pmin = v_min(pts[offs+j], tri_pmin);`	174	`tri_pmin = v_min(pts[offs+j], tri_pmin);`
175	`tri_pmax = v_max(pts[offs+j], tri_pmax);`	175	`tri_pmax = v_max(pts[offs+j], tri_pmax);`
176	`}`	176	`}`
177	`}`	177	`}`
178		178
179	`// Find the point closest to the centre.`	179	`// Find the point closest to the centre.`
180	`const Vec3f centre = tri_pmin + 0.5f*(tri_pmax-tri_pmin);`	180	`const Vec3f centre = tri_pmin + 0.5f*(tri_pmax-tri_pmin);`
181	`Vec3f centre_close;`	181	`Vec3f centre_close;`
182	`float min_dist = FLT_MAX;`	182	`float min_dist = FLT_MAX;`
183	`for(int i=0;i<N_pts;++i)`	183	`for(int i=0;i<N_pts;++i)`
184	`{`	184	`{`
185	`Vec3f v = pts[i];`	185	`Vec3f v = pts[i];`
186	`float sl = sqr_length(centre-v);`	186	`float sl = sqr_length(centre-v);`
187	`if(sl < min_dist)`	187	`if(sl < min_dist)`
188	`{`	188	`{`
189	`min_dist = sl;`	189	`min_dist = sl;`
190	`centre_close = v;`	190	`centre_close = v;`
191	`}`	191	`}`
192	`}`	192	`}`
193		193
194	`// Partition the triangles`	194	`// Partition the triangles`
195	`const float thresh = centre[0];`	195	`const float thresh = centre[0];`
196	`for(int i=0;i<N_tri;++i)`	196	`for(int i=0;i<N_tri;++i)`
197	`{`	197	`{`
198	`Vec3f p = Rot * invec[i].get_centre();`	198	`Vec3f p = Rot * invec[i].get_centre();`
199	`if( p[0] > thresh)`	199	`if( p[0] > thresh)`
200	`rvec.push_back(invec[i]);`	200	`rvec.push_back(invec[i]);`
201	`else`	201	`else`
202	`lvec.push_back(invec[i]);`	202	`lvec.push_back(invec[i]);`
203	`}`	203	`}`
204		204
205	`// If all triangles landed in one box, split them naively.`	205	`// If all triangles landed in one box, split them naively.`
206	`if(lvec.empty() \|\| rvec.empty())`	206	`if(lvec.empty() \|\| rvec.empty())`
207	`{`	207	`{`
208	`lvec.clear();`	208	`lvec.clear();`
209	`lvec.insert(lvec.end(),`	209	`lvec.insert(lvec.end(),`
210	`invec.begin(),`	210	`invec.begin(),`
211	`invec.begin()+N_tri/2);`	211	`invec.begin()+N_tri/2);`
212	`rvec.clear();`	212	`rvec.clear();`
213	`rvec.insert(rvec.end(),`	213	`rvec.insert(rvec.end(),`
214	`invec.begin()+N_tri/2,`	214	`invec.begin()+N_tri/2,`
215	`invec.end());`	215	`invec.end());`
216	`}`	216	`}`
217		217
218	`return OBox(Rot, AABox(tri_pmin, tri_pmax, centre_close));`	218	`return OBox(Rot, AABox(tri_pmin, tri_pmax, centre_close));`
219	`}`	219	`}`
220		220
221	`}`	221	`}`
222		222

Subversion Repositories gelsvn

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