Frustum sphere culling.

Do a sphere<->camera sphere and a sphere<->frustum before the box<->frustum test.

6 files changed, 303 insertions, 64 deletions

diff --git a/source/gameengine/SceneGraph/SG_BBox.cpp b/source/gameengine/SceneGraph/SG_BBox.cpp
index 1430313afaf..bc7d00327a9 100644
--- a/source/gameengine/SceneGraph/SG_BBox.cpp
+++ b/source/gameengine/SceneGraph/SG_BBox.cpp
@@ -52,6 +52,12 @@ SG_BBox::SG_BBox(const SG_BBox &other, const MT_Transform &world) :
 	m_min(world(other.m_min)),
 	m_max(world(other.m_max))
 {
+	*this += world(MT_Point3(m_min[0], m_min[1], m_max[2]));
+	*this += world(MT_Point3(m_min[0], m_max[1], m_min[2]));
+	*this += world(MT_Point3(m_min[0], m_max[1], m_max[2]));
+	*this += world(MT_Point3(m_max[0], m_min[1], m_min[2]));
+	*this += world(MT_Point3(m_max[0], m_min[1], m_max[2]));
+	*this += world(MT_Point3(m_max[0], m_max[1], m_min[2]));
 }
 
 SG_BBox::SG_BBox(const SG_BBox &other) :
@@ -64,7 +70,7 @@ SG_BBox::~ SG_BBox()
 {
 }
 
-SG_BBox& SG_BBox::operator +=(MT_Point3 &point)
+SG_BBox& SG_BBox::operator +=(const MT_Point3 &point)
 {
 	if (point[0] < m_min[0])
 		m_min[0] = point[0];
@@ -84,7 +90,7 @@ SG_BBox& SG_BBox::operator +=(MT_Point3 &point)
 	return *this;
 }
 
-SG_BBox& SG_BBox::operator += (SG_BBox &bbox)
+SG_BBox& SG_BBox::operator += (const SG_BBox &bbox)
 {
 	*this += bbox.m_min;
 	*this += bbox.m_max;
@@ -92,7 +98,7 @@ SG_BBox& SG_BBox::operator += (SG_BBox &bbox)
 	return *this;
 }
 
-SG_BBox SG_BBox::operator +(SG_BBox &bbox2) const
+SG_BBox SG_BBox::operator +(const SG_BBox &bbox2) const
 {
 	SG_BBox ret = *this;
 	ret += bbox2;
@@ -121,7 +127,14 @@ void SG_BBox::scale(const MT_Vector3& size, const MT_Point3& point)
 
 SG_BBox SG_BBox::transform(const MT_Transform &world) const
 {
-	return SG_BBox(world(m_min), world(m_max));
+	SG_BBox bbox(world(m_min), world(m_max));
+	bbox += world(MT_Point3(m_min[0], m_min[1], m_max[2]));
+	bbox += world(MT_Point3(m_min[0], m_max[1], m_min[2]));
+	bbox += world(MT_Point3(m_min[0], m_max[1], m_max[2]));
+	bbox += world(MT_Point3(m_max[0], m_min[1], m_min[2]));
+	bbox += world(MT_Point3(m_max[0], m_min[1], m_max[2]));
+	bbox += world(MT_Point3(m_max[0], m_max[1], m_min[2]));
+	return bbox;
 }
 
 bool SG_BBox::inside(const MT_Point3 &point) const
@@ -177,3 +190,64 @@ void SG_BBox::getaa(MT_Point3 *box, const MT_Transform &world) const
 	*box++ = MT_Point3(max[0], max[1], min[2]);
 	*box++ = max;
 }
+
+void SG_BBox::split(SG_BBox &left, SG_BBox &right) const
+{
+	MT_Scalar sizex = m_max[0] - m_min[0];
+	MT_Scalar sizey = m_max[1] - m_min[1];
+	MT_Scalar sizez = m_max[2] - m_min[2];
+	if (sizex < sizey)
+	{
+		if (sizey > sizez)
+		{
+			left.m_min = m_min;
+			left.m_max[0] = m_max[0];
+			left.m_max[1] = m_min[1] + sizey/2.0;
+			left.m_max[2] = m_max[2];
+			
+			right.m_min[0] = m_min[0];
+			right.m_min[1] = m_min[1] + sizey/2.0;
+			right.m_min[2] = m_min[2];
+			right.m_max = m_max;
+			std::cout << "splity" << std::endl;
+		} else {
+			left.m_min = m_min;
+			left.m_max[0] = m_max[0];
+			left.m_max[1] = m_max[1];
+			left.m_max[2] = m_min[2] + sizez/2.0;
+		
+			right.m_min[0] = m_min[0];
+			right.m_min[1] = m_min[1];
+			right.m_min[2] = m_min[2] + sizez/2.0;
+			right.m_max = m_max;
+			std::cout << "splitz" << std::endl;
+		}
+	} else {
+		if (sizex > sizez)
+		{
+			left.m_min = m_min;
+			left.m_max[0] = m_min[0] + sizex/2.0;
+			left.m_max[1] = m_max[1];
+			left.m_max[2] = m_max[2];
+			
+			right.m_min[0] = m_min[0] + sizex/2.0;
+			right.m_min[1] = m_min[1];
+			right.m_min[2] = m_min[2];
+			right.m_max = m_max;
+			std::cout << "splitx" << std::endl;
+		} else {
+			left.m_min = m_min;
+			left.m_max[0] = m_max[0];
+			left.m_max[1] = m_max[1];
+			left.m_max[2] = m_min[2] + sizez/2.0;
+		
+			right.m_min[0] = m_min[0];
+			right.m_min[1] = m_min[1];
+			right.m_min[2] = m_min[2] + sizez/2.0;
+			right.m_max = m_max;
+			std::cout << "splitz" << std::endl;
+		}
+	}
+	
+	//std::cout << "Left: " << left.m_min << " -> " << left.m_max << " Right: " << right.m_min << " -> " << right.m_max << std::endl;
+}
diff --git a/source/gameengine/SceneGraph/SG_BBox.h b/source/gameengine/SceneGraph/SG_BBox.h
index 6e3d208ea54..5a4d396faf1 100644
--- a/source/gameengine/SceneGraph/SG_BBox.h
+++ b/source/gameengine/SceneGraph/SG_BBox.h
@@ -63,13 +63,13 @@ public:
 	/**
 	 * Enlarges the bounding box to contain the specified point.
 	 */
-	SG_BBox& operator +=(MT_Point3 &point);
+	SG_BBox& operator +=(const MT_Point3 &point);
 	/**
 	 * Enlarges the bounding box to contain the specified bound box.
 	 */
-	SG_BBox& operator +=(SG_BBox &bbox);
+	SG_BBox& operator +=(const SG_BBox &bbox);
 	
-	SG_BBox operator + (SG_BBox &bbox2) const;
+	SG_BBox operator + (const SG_BBox &bbox2) const;
 #if 0
 	/**
 	 * Translates the bounding box.
@@ -124,6 +124,10 @@ public:
 	 * @param world a world transform to be applied.
 	 */
 	void getaa(MT_Point3 *box, const MT_Transform &world) const;
+	
+	void split(SG_BBox &left, SG_BBox &right) const;
+	
+	friend class SG_Tree;
 
 };
 
diff --git a/source/gameengine/SceneGraph/SG_Spatial.cpp b/source/gameengine/SceneGraph/SG_Spatial.cpp
index 633e89b3e6a..c65542c98ce 100644
--- a/source/gameengine/SceneGraph/SG_Spatial.cpp
+++ b/source/gameengine/SceneGraph/SG_Spatial.cpp
@@ -50,7 +50,7 @@ SG_Spatial(
 	m_localPosition(MT_Point3(0,0,0)),
 	m_localRotation(1,0,0,0,1,0,0,0,1),
 	m_localScaling(MT_Vector3(1.f,1.f,1.f)),
-
+	
 	m_worldPosition(MT_Point3(0,0,0)),
 	m_worldRotation(0,0,0,0,0,0,0,0,0),
 	m_worldScaling(MT_Vector3(1.f,1.f,1.f)),
@@ -74,7 +74,8 @@ SG_Spatial(
 	
 	m_parent_relation(NULL),
 	
-	m_bbox(other.m_bbox)
+	m_bbox(other.m_bbox),
+	m_radius(other.m_radius)
 {
 	// duplicate the parent relation for this object
 	m_parent_relation = other.m_parent_relation->NewCopy();
@@ -118,7 +119,8 @@ UpdateSpatialData(
 
 	for (;cit!=c_end;++cit)
 	{
-		bComputesWorldTransform = bComputesWorldTransform || (*cit)->Update(time);
+		if ((*cit)->Update(time))
+			bComputesWorldTransform = true;
 	}
 
 	// If none of the objects updated our values then we ask the
@@ -126,9 +128,9 @@ UpdateSpatialData(
 	// our world coordinates.
 
 	if (!bComputesWorldTransform)
-    {
+	{
 		ComputeWorldTransforms(parent);
-    }
+	}
 }
 
 void	SG_Spatial::ComputeWorldTransforms(const SG_Spatial *parent)
@@ -299,18 +301,26 @@ void SG_Spatial::SetBBox(SG_BBox& bbox)
 	m_bbox = bbox;
 }
 
+MT_Transform SG_Spatial::GetWorldTransform() const
+{
+	return MT_Transform(m_worldPosition, m_worldRotation.scaled(m_worldScaling[0], m_worldScaling[1], m_worldScaling[2]));
+}
+
 bool SG_Spatial::inside(const MT_Point3 &point) const
 {
-	return m_bbox.transform(MT_Transform(m_worldPosition, m_worldRotation.scaled(m_worldScaling[0], m_worldScaling[1], m_worldScaling[2]))).inside(point);
+	MT_Scalar radius = m_worldScaling[m_worldScaling.closestAxis()]*m_radius;
+	return (m_worldPosition.distance2(point) <= radius*radius) ?
+		m_bbox.transform(GetWorldTransform()).inside(point) :
+		false;
 }
 
 void SG_Spatial::getBBox(MT_Point3 *box) const
 {
-	m_bbox.get(box, MT_Transform(m_worldPosition, m_worldRotation.scaled(m_worldScaling[0], m_worldScaling[1], m_worldScaling[2])));
+	m_bbox.get(box, GetWorldTransform());
 }
 
 void SG_Spatial::getAABBox(MT_Point3 *box) const
 {
-	m_bbox.getaa(box, MT_Transform(m_worldPosition, m_worldRotation.scaled(m_worldScaling[0], m_worldScaling[1], m_worldScaling[2])));
+	m_bbox.getaa(box, GetWorldTransform());
 }
 
diff --git a/source/gameengine/SceneGraph/SG_Spatial.h b/source/gameengine/SceneGraph/SG_Spatial.h
index e029ce51919..e58e45bb451 100644
--- a/source/gameengine/SceneGraph/SG_Spatial.h
+++ b/source/gameengine/SceneGraph/SG_Spatial.h
@@ -53,16 +53,18 @@ class SG_Spatial : public SG_IObject
 
 protected:
 	MT_Point3		m_localPosition;
-	MT_Matrix3x3	m_localRotation;
+	MT_Matrix3x3		m_localRotation;
 	MT_Vector3		m_localScaling;
 
 	MT_Point3		m_worldPosition;
-	MT_Matrix3x3	m_worldRotation;
+	MT_Matrix3x3		m_worldRotation;
 	MT_Vector3		m_worldScaling;
 	
-	SG_ParentRelation * m_parent_relation;
+	SG_ParentRelation *	m_parent_relation;
+	
+	SG_BBox			m_bbox;
+	MT_Scalar		m_radius;
 	
-	SG_BBox		m_bbox;
 
 public:
 
@@ -173,6 +175,7 @@ public:
 	GetWorldScaling(
 	) const	;
 
+	MT_Transform GetWorldTransform() const;
 
 	void	ComputeWorldTransforms(		const SG_Spatial *parent);
 
@@ -184,7 +187,10 @@ public:
 	bool inside(const MT_Point3 &point) const;
 	void getBBox(MT_Point3 *box) const;
 	void getAABBox(MT_Point3 *box) const;
-
+	
+	MT_Scalar Radius() const { return m_radius; }
+	void SetRadius(MT_Scalar radius) { m_radius = radius; }
+	
 protected:
 	friend class SG_Controller;
 	
diff --git a/source/gameengine/SceneGraph/SG_Tree.cpp b/source/gameengine/SceneGraph/SG_Tree.cpp
index 417e42858f5..fcbf3d924d3 100644
--- a/source/gameengine/SceneGraph/SG_Tree.cpp
+++ b/source/gameengine/SceneGraph/SG_Tree.cpp
@@ -46,9 +46,18 @@ SG_Tree::SG_Tree(SG_Tree* left, SG_Tree* right) :
 		m_right(right),
 		m_client_object(NULL)
 {
-	m_bbox = m_left->m_bbox + m_right->m_bbox;
-	m_left->m_parent = this;
-	m_right->m_parent = this;
+	if (m_left)
+	{
+		m_bbox = m_left->m_bbox;
+		m_left->m_parent = this;
+	}
+	if (m_right)
+	{
+		m_bbox += m_right->m_bbox;
+		m_right->m_parent = this;
+	}
+	m_centre = (m_bbox.m_min + m_bbox.m_max)/2.0;
+	m_radius = (m_bbox.m_max - m_bbox.m_min).length();
 }
 	
 SG_Tree::SG_Tree(SG_Node* client) :
@@ -56,10 +65,9 @@ SG_Tree::SG_Tree(SG_Node* client) :
 		m_right(NULL),
 		m_client_object(client)
 {
-	const MT_Vector3 &scale = client->GetWorldScaling();
-	m_bbox = SG_BBox(client->BBox(), 
-		MT_Transform(client->GetWorldPosition(), 
-			client->GetWorldOrientation().scaled(scale[0], scale[1], scale[2])));
+	m_bbox = SG_BBox(client->BBox(), client->GetWorldTransform());
+	m_centre = (m_bbox.m_min + m_bbox.m_max)/2.0;
+	m_radius = (m_bbox.m_max - m_bbox.m_min).length();
 }
 
 SG_Tree::~SG_Tree() 
@@ -128,23 +136,13 @@ SG_Tree* SG_Tree::Find(SG_Node *node)
 
 void SG_Tree::get(MT_Point3 *box) const
 {
-	if (m_client_object)
-	{
-		m_client_object->getAABBox(box);
-	}
-	else
-	{
-		MT_Transform identity;
-		identity.setIdentity();
-		m_bbox.getaa(box, identity);
-	}
+	MT_Transform identity;
+	identity.setIdentity();
+	m_bbox.get(box, identity);
 }
 
 bool SG_Tree::inside(const MT_Point3 &point) const
 {
-	if (m_client_object)
-		return m_client_object->inside(point);
-
 	return m_bbox.inside(point);
 }
 
@@ -153,18 +151,20 @@ const SG_BBox& SG_Tree::BBox() const
 	return m_bbox;
 }
 
-SG_TreeFactory::SG_TreeFactory()
+void SG_Tree::SetLeft(SG_Tree *left)
 {
+	m_left = left;
+	m_bbox += left->m_bbox;
+	m_centre = (m_bbox.m_min + m_bbox.m_max)/2.0;
+	m_radius = (m_bbox.m_max - m_bbox.m_min).length();
 }
 
-SG_TreeFactory::~SG_TreeFactory()
+void SG_Tree::SetRight(SG_Tree *right)
 {
-}
-	
-void SG_TreeFactory::Add(SG_Node* client)
-{
-	if (client)
-		m_objects.push_back(new SG_Tree(client));
+	m_right = right;
+	m_bbox += right->m_bbox;
+	m_centre = (m_bbox.m_min + m_bbox.m_max)/2.0;
+	m_radius = (m_bbox.m_max - m_bbox.m_min).length();
 }
 
 /**
@@ -215,25 +215,151 @@ public:
 	}
 };
 
+SG_TreeFactory::SG_TreeFactory()
+{
+}
+
+SG_TreeFactory::~SG_TreeFactory()
+{
+}
+	
+void SG_TreeFactory::Add(SG_Node* client)
+{
+	if (client)
+		m_objects.insert(new SG_Tree(client));
+}
+
+void SG_TreeFactory::Add(SG_Tree* tree)
+{
+	m_objects.insert(tree);
+}
+
+SG_Tree* SG_TreeFactory::MakeTreeDown(SG_BBox &bbox)
+{
+	if (m_objects.size() == 0)
+		return NULL;
+	if (m_objects.size() == 1)
+		return *m_objects.begin();
+		
+	TreeSet::iterator it = m_objects.begin();
+	SG_Tree *root = *it;
+	if (m_objects.size() == 2)
+	{
+		root->SetRight(*(++it));
+		return root;
+	}
+		
+	if (m_objects.size() == 3)
+	{
+		root->SetLeft(*(++it));
+		root->SetRight(*(++it));
+		return root;
+	}
+
+	if (bbox.volume() < 1.0)
+		return MakeTreeUp();
+		
+	SG_TreeFactory lefttree;
+	SG_TreeFactory righttree;
+	
+	SG_BBox left, right;
+	int hasleft = 0, hasright = 0;
+	bbox.split(left, right);
+	
+	if (left.test(root->BBox()) == SG_BBox::INSIDE)
+	{
+		lefttree.Add(root);
+		root = NULL;
+	}
+	
+	if (root && right.test(root->BBox()) == SG_BBox::INSIDE)
+	{
+		righttree.Add(root);
+		root = NULL;
+	}
+	
+	for (++it; it != m_objects.end(); ++it)
+	{
+		switch (left.test((*it)->BBox()))
+		{
+			case SG_BBox::INSIDE:
+				// Object is inside left tree;
+				lefttree.Add(*it);
+				hasleft++;
+				break;
+			case SG_BBox::OUTSIDE:
+				righttree.Add(*it);
+				hasright++;
+				break;
+			case SG_BBox::INTERSECT:
+				if (left.inside((*it)->Client()->GetWorldPosition()))
+				{
+					lefttree.Add(*it);
+					hasleft++;
+				} else {
+					righttree.Add(*it);
+					hasright++;
+				}
+				break;
+		}
+	}
+	std::cout << "Left: " << hasleft << " Right: " << hasright << " Count: " << m_objects.size() << std::endl;
+	
+	SG_Tree *leftnode = NULL;
+	if (hasleft)
+		leftnode = lefttree.MakeTreeDown(left);
+	
+	SG_Tree *rightnode = NULL;
+	if (hasright)
+		rightnode = righttree.MakeTreeDown(right);
+		
+	if (!root)
+		root = new SG_Tree(leftnode, rightnode);
+	else
+	{
+		if (leftnode)
+			root->SetLeft(leftnode);
+		if (rightnode)
+			root->SetRight(rightnode);
+	}
+
+	return root;
+}
+
 SG_Tree* SG_TreeFactory::MakeTree()
 {
+	if (m_objects.size() < 8)
+		return MakeTreeUp();
+
+	TreeSet::iterator it = m_objects.begin();
+	SG_BBox bbox((*it)->BBox());
+	for (++it; it != m_objects.end(); ++it)
+		bbox += (*it)->BBox();
+	
+	return MakeTreeDown(bbox);
+}
+
+SG_Tree* SG_TreeFactory::MakeTreeUp()
+{
 	unsigned int num_objects = m_objects.size();
 	
 	if (num_objects < 1)
 		return NULL;
 	if (num_objects < 2)
-		return m_objects[0];
+		return *m_objects.begin();
 
 	HalfArray<SG_Tree*> sizes;
 	sizes.resize(num_objects);
 	
 	unsigned int x, y;
-	for( y = 0; y < num_objects; y++)
+	TreeSet::iterator xit, yit;
+	for( y = 0, yit = m_objects.begin(); y < num_objects; y++, ++yit)
 	{
-		sizes(y, y) = m_objects[y];
-		for( x = y+1; x < num_objects; x++)
+		sizes(y, y) = *yit;
+		xit = yit;
+		for( x = y+1, ++xit; x < num_objects; x++, ++xit)
 		{
-			sizes(x, y) = new SG_Tree(m_objects[x], m_objects[y]);
+			sizes(x, y) = new SG_Tree(*xit, *yit);
 			
 		}
 	}
@@ -246,14 +372,8 @@ SG_Tree* SG_TreeFactory::MakeTree()
 		//char temp[16];
 		for( y = 0; y < num_objects; y++)
 		{
-			/*std::cout << sizes(y, y) << " ";
-			for( unsigned int x = 0; x < y; x++)
-				std::cout << "                   "; */
-			
 			for( x = y+1; x < num_objects; x++)
 			{
-				//sprintf(temp, "%7.1f", sizes(x, y)->volume());
-				//std::cout << sizes(x, y) << "(" << temp << ") ";
 				if (sizes(x, y)->volume() < min_volume)
 				{
 					min = sizes(x, y);
@@ -262,11 +382,8 @@ SG_Tree* SG_TreeFactory::MakeTree()
 					min_volume = sizes(x, y)->volume();
 				}
 			}
-			//std::cout << std::endl;
 		}
 		
-		//std::cout << "minx, miny, minv = " << minx << ", " << miny << ", " << min_volume << std::endl;
-		
 		/* Remove other bboxes that contain the two bboxes */
 		sizes.delete_column(miny);
 		
diff --git a/source/gameengine/SceneGraph/SG_Tree.h b/source/gameengine/SceneGraph/SG_Tree.h
index 78c8411de8d..51eeec4f3e1 100644
--- a/source/gameengine/SceneGraph/SG_Tree.h
+++ b/source/gameengine/SceneGraph/SG_Tree.h
@@ -37,7 +37,7 @@
 #include "MT_Point3.h"
 #include "SG_BBox.h"
 
-#include <vector> 
+#include <set> 
 
 class SG_Node;
 
@@ -52,6 +52,8 @@ class SG_Tree
 	SG_Tree* m_right;
 	SG_Tree* m_parent;
 	SG_BBox  m_bbox;
+	MT_Point3 m_centre;
+	MT_Scalar m_radius;
 	SG_Node* m_client_object;
 public:
 	SG_Tree();
@@ -95,7 +97,23 @@ public:
 	 * Test if the given bounding box is inside this bounding box.
 	 */
 	bool inside(const MT_Point3 &point) const;
+	
+	void SetLeft(SG_Tree *left);
+	void SetRight(SG_Tree *right);
 
+	MT_Point3 Centre() const { return m_centre; }
+	MT_Scalar Radius() { return m_radius; }
+	
+	//friend class SG_TreeFactory;
+	
+	struct greater
+	{
+		bool operator()(const SG_Tree *a, const SG_Tree *b)
+		{
+			return a->volume() > b->volume();
+		}
+	};
+	
 };
 
 
@@ -108,7 +126,8 @@ public:
  */
 class SG_TreeFactory 
 {
-	std::vector<SG_Tree*> m_objects;
+	typedef std::multiset<SG_Tree*, SG_Tree::greater> TreeSet;
+	TreeSet m_objects;
 public:
 	SG_TreeFactory();
 	~SG_TreeFactory();
@@ -117,12 +136,21 @@ public:
 	 *  Add a node to be added to the tree.
 	 */
 	void Add(SG_Node* client);
+	void Add(SG_Tree* tree);
 
 	/**
 	 *  Build the tree from the set of nodes added by
 	 *  the Add method.
 	 */
+	SG_Tree* MakeTreeUp();
+	
+	/**
+	 *  Build the tree from the set of nodes top down.
+	 */
+	SG_Tree* MakeTreeDown(SG_BBox &bbox);
+	
 	SG_Tree* MakeTree();
+	
 };
 
 #endif /* __SG_BBOX_H__ */