path: root/libraries/ode-0.9/ode/src/collision_quadtreespace.cpp

/*************************************************************************
 *                                                                       *
 * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith.       *
 * All rights reserved.  Email: russ@q12.org   Web: www.q12.org          *
 *                                                                       *
 * This library is free software; you can redistribute it and/or         *
 * modify it under the terms of EITHER:                                  *
 *   (1) The GNU Lesser General Public License as published by the Free  *
 *       Software Foundation; either version 2.1 of the License, or (at  *
 *       your option) any later version. The text of the GNU Lesser      *
 *       General Public License is included with this library in the     *
 *       file LICENSE.TXT.                                               *
 *   (2) The BSD-style license that is included with this library in     *
 *       the file LICENSE-BSD.TXT.                                       *
 *                                                                       *
 * This library is distributed in the hope that it will be useful,       *
 * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files    *
 * LICENSE.TXT and LICENSE-BSD.TXT for more details.                     *
 *                                                                       *
 *************************************************************************/

// QuadTreeSpace by Erwin de Vries.

#include <ode/common.h>
#include <ode/matrix.h>
#include <ode/collision_space.h>
#include <ode/collision.h>
#include "collision_kernel.h"

#include "collision_space_internal.h"


#define AXIS0 0
#define AXIS1 1
#define UP 2

//#define DRAWBLOCKS

const int SPLITAXIS = 2;
const int SPLITS = SPLITAXIS * SPLITAXIS;

#define GEOM_ENABLED(g) (g)->gflags & GEOM_ENABLED

class Block{
public:
	dReal MinX, MaxX;
	dReal MinZ, MaxZ;

	dGeomID First;
	int GeomCount;

	Block* Parent;
	Block* Children;

	void Create(const dVector3 Center, const dVector3 Extents, Block* Parent, int Depth, Block*& Blocks);

	void Collide(void* UserData, dNearCallback* Callback);
	void Collide(dGeomID Object, dGeomID g, void* UserData, dNearCallback* Callback);

	void CollideLocal(dGeomID Object, void* UserData, dNearCallback* Callback);
	
	void AddObject(dGeomID Object);
	void DelObject(dGeomID Object);
	void Traverse(dGeomID Object);

	bool Inside(const dReal* AABB);
	
	Block* GetBlock(const dReal* AABB);
	Block* GetBlockChild(const dReal* AABB);
};


#ifdef DRAWBLOCKS
#include "..\..\Include\drawstuff\\drawstuff.h"

static void DrawBlock(Block* Block){
	dVector3 v[8];
	v[0][AXIS0] = Block->MinX;
	v[0][UP] = REAL(-1.0);
	v[0][AXIS1] = Block->MinZ;
	
	v[1][AXIS0] = Block->MinX;
	v[1][UP] = REAL(-1.0);
	v[1][AXIS1] = Block->MaxZ;
	
	v[2][AXIS0] = Block->MaxX;
	v[2][UP] = REAL(-1.0);
	v[2][AXIS1] = Block->MinZ;
	
	v[3][AXIS0] = Block->MaxX;
	v[3][UP] = REAL(-1.0);
	v[3][AXIS1] = Block->MaxZ;
	
	v[4][AXIS0] = Block->MinX;
	v[4][UP] = REAL(1.0);
	v[4][AXIS1] = Block->MinZ;
	
	v[5][AXIS0] = Block->MinX;
	v[5][UP] = REAL(1.0);
	v[5][AXIS1] = Block->MaxZ;
	
	v[6][AXIS0] = Block->MaxX;
	v[6][UP] = REAL(1.0);
	v[6][AXIS1] = Block->MinZ;
	
	v[7][AXIS0] = Block->MaxX;
	v[7][UP] = REAL(1.0);
	v[7][AXIS1] = Block->MaxZ;
	
	// Bottom
	dsDrawLine(v[0], v[1]);
	dsDrawLine(v[1], v[3]);
	dsDrawLine(v[3], v[2]);
	dsDrawLine(v[2], v[0]);
	
	// Top
	dsDrawLine(v[4], v[5]);
	dsDrawLine(v[5], v[7]);
	dsDrawLine(v[7], v[6]);
	dsDrawLine(v[6], v[4]);
	
	// Sides
	dsDrawLine(v[0], v[4]);
	dsDrawLine(v[1], v[5]);
	dsDrawLine(v[2], v[6]);
	dsDrawLine(v[3], v[7]);
}
#endif	//DRAWBLOCKS


void Block::Create(const dVector3 Center, const dVector3 Extents, Block* Parent, int Depth, Block*& Blocks){
	GeomCount = 0;
	First = 0;

	MinX = Center[AXIS0] - Extents[AXIS0];
	MaxX = Center[AXIS0] + Extents[AXIS0];

	MinZ = Center[AXIS1] - Extents[AXIS1];
	MaxZ = Center[AXIS1] + Extents[AXIS1];

	this->Parent = Parent;
	if (Depth > 0){
		Children = Blocks;
		Blocks += SPLITS;

		dVector3 ChildExtents;
		ChildExtents[AXIS0] = Extents[AXIS0] / SPLITAXIS;
		ChildExtents[AXIS1] = Extents[AXIS1] / SPLITAXIS;
		ChildExtents[UP] = Extents[UP];

		for (int i = 0; i < SPLITAXIS; i++){
			for (int j = 0; j < SPLITAXIS; j++){
				int Index = i * SPLITAXIS + j;

				dVector3 ChildCenter;
				ChildCenter[AXIS0] = Center[AXIS0] - Extents[AXIS0] + ChildExtents[AXIS0] + i * (ChildExtents[AXIS0] * 2);
				ChildCenter[AXIS1] = Center[AXIS1] - Extents[AXIS1] + ChildExtents[AXIS1] + j * (ChildExtents[AXIS1] * 2);
				ChildCenter[UP] = Center[UP];
				
				Children[Index].Create(ChildCenter, ChildExtents, this, Depth - 1, Blocks);
			}
		}
	}
	else Children = 0;
}

void Block::Collide(void* UserData, dNearCallback* Callback){
#ifdef DRAWBLOCKS
	DrawBlock(this);
#endif
	// Collide the local list
	dxGeom* g = First;
	while (g){
		if (GEOM_ENABLED(g)){
			Collide(g, g->next, UserData, Callback);
		}
		g = g->next;
	}

	// Recurse for children
	if (Children){
		for (int i = 0; i < SPLITS; i++){
			if (Children[i].GeomCount <= 1){	// Early out
				continue;
			}
			Children[i].Collide(UserData, Callback);
		}
	}
}

void Block::Collide(dxGeom* g1, dxGeom* g2, void* UserData, dNearCallback* Callback){
#ifdef DRAWBLOCKS
	DrawBlock(this);
#endif
	// Collide against local list
	while (g2){
		if (GEOM_ENABLED(g2)){
			collideAABBs (g1, g2, UserData, Callback);
		}
		g2 = g2->next;
	}

	// Collide against children
	if (Children){
		for (int i = 0; i < SPLITS; i++){
			// Early out for empty blocks
			if (Children[i].GeomCount == 0){
				continue;
			}

			// Does the geom's AABB collide with the block?
			// Dont do AABB tests for single geom blocks.
			if (Children[i].GeomCount == 1 && Children[i].First){
				//
			}
			else if (true){
				if (g1->aabb[AXIS0 * 2 + 0] > Children[i].MaxX ||
					g1->aabb[AXIS0 * 2 + 1] < Children[i].MinX ||
					g1->aabb[AXIS1 * 2 + 0] > Children[i].MaxZ ||
					g1->aabb[AXIS1 * 2 + 1] < Children[i].MinZ) continue;
			}
			Children[i].Collide(g1, Children[i].First, UserData, Callback);
		}
	}
}

void Block::CollideLocal(dxGeom* g1, void* UserData, dNearCallback* Callback){
	// Collide against local list
	dxGeom* g2 = First;
	while (g2){
		if (GEOM_ENABLED(g2)){
			collideAABBs (g1, g2, UserData, Callback);
		}
		g2 = g2->next;
	}
}

void Block::AddObject(dGeomID Object){
	// Add the geom
	Object->next = First;
	First = Object;
	Object->tome = (dxGeom**)this;

	// Now traverse upwards to tell that we have a geom
	Block* Block = this;
	do{
		Block->GeomCount++;
		Block = Block->Parent;
	}
	while (Block);
}

void Block::DelObject(dGeomID Object){
	// Del the geom
	dxGeom* g = First;
	dxGeom* Last = 0;
	while (g){
		if (g == Object){
			if (Last){
				Last->next = g->next;
			}
			else First = g->next;

			break;
		}
		Last = g;
		g = g->next;
	}

	Object->tome = 0;

	// Now traverse upwards to tell that we have lost a geom
	Block* Block = this;
	do{
		Block->GeomCount--;
		Block = Block->Parent;
	}
	while (Block);
}

void Block::Traverse(dGeomID Object){
	Block* NewBlock = GetBlock(Object->aabb);

	if (NewBlock != this){
		// Remove the geom from the old block and add it to the new block.
		// This could be more optimal, but the loss should be very small.
		DelObject(Object);
		NewBlock->AddObject(Object);
	}
}

bool Block::Inside(const dReal* AABB){
	return AABB[AXIS0 * 2 + 0] >= MinX && AABB[AXIS0 * 2 + 1] <= MaxX && AABB[AXIS1 * 2 + 0] >= MinZ && AABB[AXIS1 * 2 + 1] <= MaxZ;
}

Block* Block::GetBlock(const dReal* AABB){
	if (Inside(AABB)){
		return GetBlockChild(AABB);	// Child or this will have a good block
	}
	else if (Parent){
		return Parent->GetBlock(AABB);	// Parent has a good block
	}
	else return this;	// We are at the root, so we have little choice
}

Block* Block::GetBlockChild(const dReal* AABB){
	if (Children){
		for (int i = 0; i < SPLITS; i++){
			if (Children[i].Inside(AABB)){
				return Children[i].GetBlockChild(AABB);	// Child will have good block
			}
		}
	}
	return this;	// This is the best block
}

//****************************************************************************
// quadtree space

struct dxQuadTreeSpace : public dxSpace{
	Block* Blocks;	// Blocks[0] is the root

	dArray<dxGeom*> DirtyList;

	dxQuadTreeSpace(dSpaceID _space, dVector3 Center, dVector3 Extents, int Depth);
	~dxQuadTreeSpace();

	dxGeom* getGeom(int i);
	
	void add(dxGeom* g);
	void remove(dxGeom* g);
	void dirty(dxGeom* g);

	void computeAABB();
	
	void cleanGeoms();
	void collide(void* UserData, dNearCallback* Callback);
	void collide2(void* UserData, dxGeom* g1, dNearCallback* Callback);

	// Temp data
	Block* CurrentBlock;	// Only used while enumerating
	int* CurrentChild;	// Only used while enumerating
	int CurrentLevel;	// Only used while enumerating
	dxGeom* CurrentObject;	// Only used while enumerating
	int CurrentIndex;
};

dxQuadTreeSpace::dxQuadTreeSpace(dSpaceID _space, dVector3 Center, dVector3 Extents, int Depth) : dxSpace(_space){
	type = dQuadTreeSpaceClass;

	int BlockCount = 0;
	for (int i = 0; i <= Depth; i++){
		BlockCount += (int)pow((dReal)SPLITS, i);
	}

	Blocks = (Block*)dAlloc(BlockCount * sizeof(Block));
	Block* Blocks = this->Blocks + 1;	// This pointer gets modified!

	this->Blocks[0].Create(Center, Extents, 0, Depth, Blocks);

	CurrentBlock = 0;
	CurrentChild = (int*)dAlloc((Depth + 1) * sizeof(int));
	CurrentLevel = 0;
	CurrentObject = 0;
	CurrentIndex = -1;

	// Init AABB. We initialize to infinity because it is not illegal for an object to be outside of the tree. Its simply inserted in the root block
	aabb[0] = -dInfinity;
	aabb[1] = dInfinity;
	aabb[2] = -dInfinity;
	aabb[3] = dInfinity;
	aabb[4] = -dInfinity;
	aabb[5] = dInfinity;
}

dxQuadTreeSpace::~dxQuadTreeSpace(){
	int Depth = 0;
	Block* Current = &Blocks[0];
	while (Current){
		Depth++;
		Current = Current->Children;
	}

	int BlockCount = 0;
	for (int i = 0; i < Depth; i++){
		BlockCount += (int)pow((dReal)SPLITS, i);
	}

	dFree(Blocks, BlockCount * sizeof(Block));
	dFree(CurrentChild, (Depth + 1) * sizeof(int));
}

dxGeom* dxQuadTreeSpace::getGeom(int Index){
	dUASSERT(Index >= 0 && Index < count, "index out of range");

	//@@@
	dDebug (0,"dxQuadTreeSpace::getGeom() not yet implemented");

	return 0;

	// This doesnt work

	/*if (CurrentIndex == Index){
		// Loop through all objects in the local list
CHILDRECURSE:
		if (CurrentObject){
			dGeomID g = CurrentObject;
			CurrentObject = CurrentObject->next;
			CurrentIndex++;
		
#ifdef DRAWBLOCKS
			DrawBlock(CurrentBlock);
#endif	//DRAWBLOCKS
			return g;
		}
		else{
			// Now lets loop through our children. Starting at index 0.
			if (CurrentBlock->Children){
				CurrentChild[CurrentLevel] = 0;
PARENTRECURSE:
				for (int& i = CurrentChild[CurrentLevel]; i < SPLITS; i++){
					if (CurrentBlock->Children[i].GeomCount == 0){
						continue;
					}
					CurrentBlock = &CurrentBlock->Children[i];
					CurrentObject = CurrentBlock->First;
				
					i++;
				
					CurrentLevel++;
					goto CHILDRECURSE;
				}
			}
		}
		
		// Now lets go back to the parent so it can continue processing its other children.
		if (CurrentBlock->Parent){
			CurrentBlock = CurrentBlock->Parent;
			CurrentLevel--;
			goto PARENTRECURSE;
		}
	}
	else{
		CurrentBlock = &Blocks[0];
		CurrentLevel = 0;
		CurrentObject = CurrentObject;
		CurrentIndex = 0;

		// Other states are already set
		CurrentObject = CurrentBlock->First;
	}


	if (current_geom && current_index == Index - 1){
		//current_geom = current_geom->next; // next
		current_index = Index;
		return current_geom;
	}
	else for (int i = 0; i < Index; i++){	// this will be verrrrrrry slow
		getGeom(i);
	}*/

	return 0;
}

void dxQuadTreeSpace::add(dxGeom* g){
	CHECK_NOT_LOCKED (this);
	dAASSERT(g);
	dUASSERT(g->parent_space == 0 && g->next == 0, "geom is already in a space");

	g->gflags |= GEOM_DIRTY | GEOM_AABB_BAD;
	DirtyList.push(g);

	// add
	g->parent_space = this;
	Blocks[0].GetBlock(g->aabb)->AddObject(g);	// Add to best block
	count++;
	
	// enumerator has been invalidated
	current_geom = 0;
	
	dGeomMoved(this);
}

void dxQuadTreeSpace::remove(dxGeom* g){
	CHECK_NOT_LOCKED(this);
	dAASSERT(g);
	dUASSERT(g->parent_space == this,"object is not in this space");
	
	// remove
	((Block*)g->tome)->DelObject(g);
	count--;

	for (int i = 0; i < DirtyList.size(); i++){
		if (DirtyList[i] == g){
			DirtyList.remove(i);
			// (mg) there can be multiple instances of a dirty object on stack  be sure to remove ALL and not just first, for this we decrement i
			--i;
		}
	}
	
	// safeguard
	g->next = 0;
	g->tome = 0;
	g->parent_space = 0;
	
	// enumerator has been invalidated
	current_geom = 0;
	
	// the bounding box of this space (and that of all the parents) may have
	// changed as a consequence of the removal.
	dGeomMoved(this);
}

void dxQuadTreeSpace::dirty(dxGeom* g){
	DirtyList.push(g);
}

void dxQuadTreeSpace::computeAABB(){
	//
}

void dxQuadTreeSpace::cleanGeoms(){
	// compute the AABBs of all dirty geoms, and clear the dirty flags
	lock_count++;
	
	for (int i = 0; i < DirtyList.size(); i++){
		dxGeom* g = DirtyList[i];
		if (IS_SPACE(g)){
			((dxSpace*)g)->cleanGeoms();
		}
		g->recomputeAABB();
		g->gflags &= (~(GEOM_DIRTY|GEOM_AABB_BAD));

		((Block*)g->tome)->Traverse(g);
	}
	DirtyList.setSize(0);

	lock_count--;
}

void dxQuadTreeSpace::collide(void* UserData, dNearCallback* Callback){
  dAASSERT(Callback);

  lock_count++;
  cleanGeoms();

  Blocks[0].Collide(UserData, Callback);

  lock_count--;
}

void dxQuadTreeSpace::collide2(void* UserData, dxGeom* g1, dNearCallback* Callback){
  dAASSERT(g1 && Callback);

  lock_count++;
  cleanGeoms();
  g1->recomputeAABB();

  if (g1->parent_space == this){
	  // The block the geom is in
	  Block* CurrentBlock = (Block*)g1->tome;
	  
	  // Collide against block and its children
	  CurrentBlock->Collide(g1, CurrentBlock->First, UserData, Callback);
	  
	  // Collide against parents
	  while (true){
		  CurrentBlock = CurrentBlock->Parent;
		  if (!CurrentBlock){
			  break;
		  }
		  CurrentBlock->CollideLocal(g1, UserData, Callback);
	  }
  }
  else Blocks[0].Collide(g1, Blocks[0].First, UserData, Callback);

  lock_count--;
}

dSpaceID dQuadTreeSpaceCreate(dxSpace* space, dVector3 Center, dVector3 Extents, int Depth){
	return new dxQuadTreeSpace(space, Center, Extents, Depth);
}