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

Subversion Repositories gelsvn

(root)/trunk/src/Geometry/OBox.cpp – Rev 460 → 595