PageRenderTime 62ms CodeModel.GetById 15ms app.highlight 42ms RepoModel.GetById 1ms app.codeStats 0ms

/indra/llmath/llsphere.cpp

https://bitbucket.org/lindenlab/viewer-beta/
C++ | 371 lines | 235 code | 39 blank | 97 comment | 50 complexity | 9b10fb4d5f8ff8ad553e69c8dbfcd611 MD5 | raw file
  1/** 
  2 * @file llsphere.cpp
  3 * @author Andrew Meadows
  4 * @brief Simple line class that can compute nearest approach between two lines
  5 *
  6 * $LicenseInfo:firstyear=2007&license=viewerlgpl$
  7 * Second Life Viewer Source Code
  8 * Copyright (C) 2010, Linden Research, Inc.
  9 * 
 10 * This library is free software; you can redistribute it and/or
 11 * modify it under the terms of the GNU Lesser General Public
 12 * License as published by the Free Software Foundation;
 13 * version 2.1 of the License only.
 14 * 
 15 * This library is distributed in the hope that it will be useful,
 16 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 17 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 18 * Lesser General Public License for more details.
 19 * 
 20 * You should have received a copy of the GNU Lesser General Public
 21 * License along with this library; if not, write to the Free Software
 22 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
 23 * 
 24 * Linden Research, Inc., 945 Battery Street, San Francisco, CA  94111  USA
 25 * $/LicenseInfo$
 26 */
 27
 28#include "linden_common.h"
 29
 30#include "llsphere.h"
 31
 32LLSphere::LLSphere()
 33:	mCenter(0.f, 0.f, 0.f),
 34	mRadius(0.f)
 35{ }
 36
 37LLSphere::LLSphere( const LLVector3& center, F32 radius)
 38{
 39	set(center, radius);
 40}
 41
 42void LLSphere::set( const LLVector3& center, F32 radius )
 43{
 44	mCenter = center;
 45	setRadius(radius);
 46}
 47
 48void LLSphere::setCenter( const LLVector3& center)
 49{
 50	mCenter = center;
 51}
 52
 53void LLSphere::setRadius( F32 radius)
 54{
 55	if (radius < 0.f)
 56	{
 57		radius = -radius;
 58	}
 59	mRadius = radius;
 60}
 61	
 62const LLVector3& LLSphere::getCenter() const
 63{
 64	return mCenter;
 65}
 66
 67F32 LLSphere::getRadius() const
 68{
 69	return mRadius;
 70}
 71
 72// returns 'TRUE' if this sphere completely contains other_sphere
 73BOOL LLSphere::contains(const LLSphere& other_sphere) const
 74{
 75	F32 separation = (mCenter - other_sphere.mCenter).length();
 76	return (mRadius >= separation + other_sphere.mRadius) ? TRUE : FALSE;
 77}
 78
 79// returns 'TRUE' if this sphere completely contains other_sphere
 80BOOL LLSphere::overlaps(const LLSphere& other_sphere) const
 81{
 82	F32 separation = (mCenter - other_sphere.mCenter).length();
 83	return (separation <= mRadius + other_sphere.mRadius) ? TRUE : FALSE;
 84}
 85
 86// returns overlap
 87// negative overlap is closest approach
 88F32 LLSphere::getOverlap(const LLSphere& other_sphere) const
 89{
 90	// separation is distance from other_sphere's edge and this center
 91	return (mCenter - other_sphere.mCenter).length() - mRadius - other_sphere.mRadius;
 92}
 93
 94bool LLSphere::operator==(const LLSphere& rhs) const
 95{
 96	// TODO? -- use approximate equality for centers?
 97	return (mRadius == rhs.mRadius
 98			&& mCenter == rhs.mCenter);
 99}
100
101std::ostream& operator<<( std::ostream& output_stream, const LLSphere& sphere)
102{
103	output_stream << "{center=" << sphere.mCenter << "," << "radius=" << sphere.mRadius << "}";
104	return output_stream;
105}
106
107// static 
108// removes any spheres that are contained in others
109void LLSphere::collapse(std::vector<LLSphere>& sphere_list)
110{
111	std::vector<LLSphere>::iterator first_itr = sphere_list.begin();
112	while (first_itr != sphere_list.end())
113	{
114		bool delete_from_front = false;
115
116		std::vector<LLSphere>::iterator second_itr = first_itr;
117		++second_itr;
118		while (second_itr != sphere_list.end())
119		{
120			if (second_itr->contains(*first_itr))
121			{
122				delete_from_front = true;
123				break;
124			}
125			else if (first_itr->contains(*second_itr))
126			{
127				sphere_list.erase(second_itr++);
128			}
129			else
130			{
131				++second_itr;
132			}
133		}
134
135		if (delete_from_front)
136		{
137			sphere_list.erase(first_itr++);
138		}
139		else
140		{
141			++first_itr;
142		}
143	}
144}
145
146// static
147// returns the bounding sphere that contains both spheres
148LLSphere LLSphere::getBoundingSphere(const LLSphere& first_sphere, const LLSphere& second_sphere)
149{
150	LLVector3 direction = second_sphere.mCenter - first_sphere.mCenter;
151
152	// HACK -- it is possible to get enough floating point error in the 
153	// other getBoundingSphere() method that we have to add some slop
154	// at the end.  Unfortunately, this breaks the link-order invarience
155	// for the linkability tests... unless we also apply the same slop
156	// here.
157	F32 half_milimeter = 0.0005f;
158
159	F32 distance = direction.length();
160	if (0.f == distance)
161	{
162		direction.setVec(1.f, 0.f, 0.f);
163	}
164	else
165	{
166		direction.normVec();
167	}
168	// the 'edge' is measured from the first_sphere's center
169	F32 max_edge = 0.f;
170	F32 min_edge = 0.f;
171
172	max_edge = llmax(max_edge + first_sphere.getRadius(), max_edge + distance + second_sphere.getRadius() + half_milimeter);
173	min_edge = llmin(min_edge - first_sphere.getRadius(), min_edge + distance - second_sphere.getRadius() - half_milimeter);
174	F32 radius = 0.5f * (max_edge - min_edge);
175	LLVector3 center = first_sphere.mCenter + (0.5f * (max_edge + min_edge)) * direction;
176	return LLSphere(center, radius);
177}
178
179// static
180// returns the bounding sphere that contains an arbitrary set of spheres
181LLSphere LLSphere::getBoundingSphere(const std::vector<LLSphere>& sphere_list)
182{
183	// this algorithm can get relatively inaccurate when the sphere 
184	// collection is 'small' (contained within a bounding sphere of about 
185	// 2 meters or less)
186	// TODO -- improve the accuracy for small collections of spheres
187
188	LLSphere bounding_sphere( LLVector3(0.f, 0.f, 0.f), 0.f );
189	S32 sphere_count = sphere_list.size();
190	if (1 == sphere_count)
191	{
192		// trivial case -- single sphere
193		std::vector<LLSphere>::const_iterator sphere_itr = sphere_list.begin();
194		bounding_sphere = *sphere_itr;
195	}
196	else if (2 == sphere_count)
197	{
198		// trivial case -- two spheres
199		std::vector<LLSphere>::const_iterator first_sphere = sphere_list.begin();
200		std::vector<LLSphere>::const_iterator second_sphere = first_sphere;
201		++second_sphere;
202		bounding_sphere = LLSphere::getBoundingSphere(*first_sphere, *second_sphere);
203	}
204	else if (sphere_count > 0)
205	{
206		// non-trivial case -- we will approximate the solution
207		//
208		// NOTE -- there is a fancy/fast way to do this for large 
209		// numbers of arbirary N-dimensional spheres -- you can look it
210		// up on the net.  We're dealing with 3D spheres at collection
211		// sizes of 256 spheres or smaller, so we just use this
212		// brute force method.
213
214		// TODO -- perhaps would be worthwile to test for the solution where
215		// the largest spanning radius just happens to work.  That is, where
216		// there are really two spheres that determine the bounding sphere,
217		// and all others are contained therein.
218
219		// compute the AABB
220		std::vector<LLSphere>::const_iterator first_itr = sphere_list.begin();
221		LLVector3 max_corner = first_itr->getCenter() + first_itr->getRadius() * LLVector3(1.f, 1.f, 1.f);
222		LLVector3 min_corner = first_itr->getCenter() - first_itr->getRadius() * LLVector3(1.f, 1.f, 1.f);
223		{
224			std::vector<LLSphere>::const_iterator sphere_itr = sphere_list.begin();
225			for (++sphere_itr; sphere_itr != sphere_list.end(); ++sphere_itr)
226			{
227				LLVector3 center = sphere_itr->getCenter();
228				F32 radius = sphere_itr->getRadius();
229				for (S32 i=0; i<3; ++i)
230				{
231					if (center.mV[i] + radius > max_corner.mV[i])
232					{
233						max_corner.mV[i] = center.mV[i] + radius;
234					}
235					if (center.mV[i] - radius < min_corner.mV[i])
236					{
237						min_corner.mV[i] = center.mV[i] - radius;
238					}
239				}
240			}
241		}
242
243		// get the starting center and radius from the AABB
244		LLVector3 diagonal = max_corner - min_corner;
245		F32 bounding_radius = 0.5f * diagonal.length();
246		LLVector3 bounding_center = 0.5f * (max_corner + min_corner);
247
248		// compute the starting step-size
249		F32 minimum_radius = 0.5f * llmin(diagonal.mV[VX], llmin(diagonal.mV[VY], diagonal.mV[VZ]));
250		F32 step_length = bounding_radius - minimum_radius;
251		S32 step_count = 0;
252		S32 max_step_count = 12;
253		F32 half_milimeter = 0.0005f;
254
255		// wander the center around in search of tighter solutions
256		S32 last_dx = 2;	// 2 is out of bounds --> no match
257		S32 last_dy = 2;
258		S32 last_dz = 2;
259
260		while (step_length > half_milimeter
261				&& step_count < max_step_count)
262		{
263			// the algorithm for testing the maximum radius could be expensive enough
264			// that it makes sense to NOT duplicate testing when possible, so we keep
265			// track of where we last tested, and only test the new points
266
267			S32 best_dx = 0;
268			S32 best_dy = 0;
269			S32 best_dz = 0;
270
271			// sample near the center of the box
272			bool found_better_center = false;
273			for (S32 dx = -1; dx < 2; ++dx)
274			{
275				for (S32 dy = -1; dy < 2; ++dy)
276				{
277					for (S32 dz = -1; dz < 2; ++dz)
278					{
279						if (dx == 0 && dy == 0 && dz == 0)
280						{
281							continue;
282						}
283
284						// count the number of indecies that match the last_*'s
285						S32 match_count = 0;
286						if (last_dx == dx) ++match_count;
287						if (last_dy == dy) ++match_count;
288						if (last_dz == dz) ++match_count;
289						if (match_count == 2)
290						{
291							// we've already tested this point
292							continue;
293						}
294
295						LLVector3 center = bounding_center;
296						center.mV[VX] += (F32) dx * step_length;
297						center.mV[VY] += (F32) dy * step_length;
298						center.mV[VZ] += (F32) dz * step_length;
299
300						// compute the radius of the bounding sphere
301						F32 max_radius = 0.f;
302						std::vector<LLSphere>::const_iterator sphere_itr;
303						for (sphere_itr = sphere_list.begin(); sphere_itr != sphere_list.end(); ++sphere_itr)
304						{
305							F32 radius = (sphere_itr->getCenter() - center).length() + sphere_itr->getRadius();
306							if (radius > max_radius)
307							{
308								max_radius = radius;
309							}
310						}
311						if (max_radius < bounding_radius)
312						{
313							best_dx = dx;
314							best_dy = dy;
315							best_dz = dz;
316							bounding_center = center;
317							bounding_radius = max_radius;
318							found_better_center = true;
319						}
320					}
321				}
322			}
323			if (found_better_center)
324			{
325				// remember where we came from so we can avoid retesting
326				last_dx = -best_dx;
327				last_dy = -best_dy;
328				last_dz = -best_dz;
329			}
330			else
331			{
332				// reduce the step size
333				step_length *= 0.5f;
334				//++step_count;
335				// reset the last_*'s
336				last_dx = 2;	// 2 is out of bounds --> no match
337				last_dy = 2;
338				last_dz = 2;
339			}
340		}
341
342		// HACK -- it is possible to get enough floating point error for the
343		// bounding sphere to too small on the order of 10e-6, but we only need
344		// it to be accurate to within about half a millimeter
345		bounding_radius += half_milimeter;
346
347		// this algorithm can get relatively inaccurate when the sphere 
348		// collection is 'small' (contained within a bounding sphere of about 
349		// 2 meters or less)
350		// TODO -- fix this
351		/* debug code
352		{
353			std::vector<LLSphere>::const_iterator sphere_itr;
354			for (sphere_itr = sphere_list.begin(); sphere_itr != sphere_list.end(); ++sphere_itr)
355			{
356				F32 radius = (sphere_itr->getCenter() - bounding_center).length() + sphere_itr->getRadius();
357				if (radius + 0.1f > bounding_radius)
358				{
359					std::cout << " rad = " << radius << "  bounding - rad = " << (bounding_radius - radius) << std::endl;
360				}
361			}
362			std::cout << "\n" << std::endl;
363		}
364		*/ 
365
366		bounding_sphere.set(bounding_center, bounding_radius);
367	}
368	return bounding_sphere;
369}
370
371