[ODE] geom tracker

[Fig TaTam]

This is a multi-part message in MIME format.

------=_NextPart_000_766_5989_6d8c
Content-Type: text/plain; format=flowed

Hi. I wrote some debugging functions for ODE's collision detection system. 
These functions keep track of all the created geoms and spaces. Then you can 
use my "write_geoms" function which will write to a text file data about all 
the geoms. Here is sample code:

dSpaceID sp1 = dSimpleSpaceCreate(NULL, "sp1");
dSpaceID sp2 = dSimpleSpaceCreate(sp1, "sp2");

dGeomID ge1 = dCreateCCylinder (sp1, 5, 2, "cyl");
dGeomID ge2 = dCreateSphere (sp2, 5, "sph");
dGeomID ray = dCreateRay (sp2, 10, "my ray");
dGeomID pln = dCreatePlane (NULL, 1, 2, 3, 4, "floor");

write_geoms(log_file);

I overloaded creation functions to take char string as another input 
parameter, that string will be printed with each geom. So the output will 
look like this:

SimpleSpace_0 [sp1]
	CCylinder_0 [cyl] pos(0.000000, 0.000000, 0.000000), r=5.000000, l=2.000000
	SimpleSpace_1 [sp2]
		Ray_0 [my ray] pos(0.000000, 0.000000, 0.000000), l=10.000000
		Sphere_0 [sph] pos(0.000000, 0.000000, 0.000000), r=5.000000
Plane_0 [floor] a=0.267261, b=0.534522, c=0.801784, d=1.069045

Every geom that's part of a space is listed below that space and indented by 
1 tab. All the comments that are passed into geom creating function are 
displayed in [] brackets. Right now there's a 60 character limit on comment 
size. Position of geom is in pos(..)

This code may come in handy if you create lots of geoms and spaces at 
runtime and something doesn't work.

_________________________________________________________________
The new MSN 8: advanced junk mail protection and 2 months FREE*  
http://join.msn.com/?page=features/junkmail

------=_NextPart_000_766_5989_6d8c
Content-Type: text/plain; name="GeomTracker.h"; format=flowed
Content-Transfer-Encoding: 8bit
Content-Disposition: attachment; filename="GeomTracker.h"

#ifndef GEOM_TRACKER_H
#define GEOM_TRACKER_H

#include <ode/ode.h>

// if not defined, the functions below will only call default ODE functions
// without any extra "geom tracking" code that I wrote
#define USE_GEOM_TRACKER

void write_geoms(FILE *fp);

dGeomID dCreateSphere (dSpaceID space, dReal radius, char *buff);
dGeomID dCreateBox (dSpaceID space, dReal lx, dReal ly, dReal lz, char 
*buff);
dGeomID dCreateCCylinder (dSpaceID space, dReal radius, dReal length, char 
*buff);
dGeomID dCreatePlane (dSpaceID space, dReal a, dReal b, dReal c, dReal d, 
char *buff);
dGeomID dCreateRay (dSpaceID space, dReal length, char *buff);
dxSpace *dHashSpaceCreate (dxSpace *space, char *buff);
dxSpace *dSimpleSpaceCreate (dxSpace *space, char *buff);

void dSpaceDestroy2 (dxSpace *space);
void dGeomDestroy2 (dGeomID geom);

#endif

------=_NextPart_000_766_5989_6d8c
Content-Type: text/plain; name="GeomTracker.cpp"; format=flowed
Content-Transfer-Encoding: 8bit
Content-Disposition: attachment; filename="GeomTracker.cpp"

#include "GeomTracker.h"

#ifdef USE_GEOM_TRACKER

#include <vector>
#include <assert.h>

using namespace std;

#define GD_TEXT_SIZE 60

struct GeomData
{
	dGeomID geom_id;
	char text[GD_TEXT_SIZE];
	int num;
	bool marked;
};

vector<GeomData> geom_array;

void dSpaceDestroy2 (dxSpace *space)
{
	dGeomID geom = (dGeomID)space;
	for(int i=0; i<geom_array.size(); i++)
		if(geom_array[i].geom_id == geom)
		{
			geom_array.erase(&geom_array[i]);
			break;
		}
	dSpaceDestroy(space);
}

void dGeomDestroy2 (dGeomID geom)
{
	for(int i=0; i<geom_array.size(); i++)
		if(geom_array[i].geom_id == geom)
		{
			geom_array.erase(&geom_array[i]);
			break;
		}
	dGeomDestroy(geom);
}

dGeomID dCreateSphere (dSpaceID space, dReal radius, char *buff)
{
	static int sphere_num = 0;
	GeomData geom_data;
	geom_data.geom_id = dCreateSphere(space, radius);
	strncpy(geom_data.text, buff, GD_TEXT_SIZE);
	geom_data.num = sphere_num++;
	geom_array.push_back(geom_data);
	return geom_data.geom_id;
}

dGeomID dCreateBox (dSpaceID space, dReal lx, dReal ly, dReal lz, char 
*buff)
{
	static int box_num = 0;
	GeomData geom_data;
	geom_data.geom_id = dCreateBox(space, lx, ly, lz);
	strncpy(geom_data.text, buff, GD_TEXT_SIZE);
	geom_data.num = box_num++;
	geom_array.push_back(geom_data);
	return geom_data.geom_id;
}

dGeomID dCreateCCylinder (dSpaceID space, dReal radius, dReal length, char 
*buff)
{
	static int ccyl_num = 0;
	GeomData geom_data;
	geom_data.geom_id = dCreateCCylinder(space, radius, length);
	strncpy(geom_data.text, buff, GD_TEXT_SIZE);
	geom_data.num = ccyl_num++;
	geom_array.push_back(geom_data);
	return geom_data.geom_id;
}

dGeomID dCreatePlane (dSpaceID space, dReal a, dReal b, dReal c, dReal d, 
char *buff)
{
	static int plane_num = 0;
	GeomData geom_data;
	geom_data.geom_id = dCreatePlane(space, a, b, c, d);
	strncpy(geom_data.text, buff, GD_TEXT_SIZE);
	geom_data.num = plane_num++;
	geom_array.push_back(geom_data);
	return geom_data.geom_id;
}

dGeomID dCreateRay (dSpaceID space, dReal length, char *buff)
{
	static int ray_num = 0;
	GeomData geom_data;
	geom_data.geom_id = dCreateRay(space, length);
	strncpy(geom_data.text, buff, GD_TEXT_SIZE);
	geom_data.num = ray_num++;
	geom_array.push_back(geom_data);
	return geom_data.geom_id;
}

dxSpace *dHashSpaceCreate (dxSpace *space, char *buff)
{
	static int hash_num = 0;
	GeomData geom_data;
	geom_data.geom_id = (dGeomID)dHashSpaceCreate(space);
	strncpy(geom_data.text, buff, GD_TEXT_SIZE);
	geom_data.num = hash_num++;
	geom_array.push_back(geom_data);
	return (dSpaceID)geom_data.geom_id;
}

dxSpace *dSimpleSpaceCreate (dxSpace *space, char *buff)
{
	static int simple_num = 0;
	GeomData geom_data;
	geom_data.geom_id = (dGeomID)dSimpleSpaceCreate(space);
	strncpy(geom_data.text, buff, GD_TEXT_SIZE);
	geom_data.num = simple_num++;
	geom_array.push_back(geom_data);
	return (dSpaceID)geom_data.geom_id;
}

struct SpaceTree;
struct SpaceTree
{
	GeomData *geom_dat;
	vector<SpaceTree> sub_tree;
};

void recursive_fill_tree(vector<SpaceTree> &top_tree, vector<SpaceTree> 
&sub_tree, SpaceTree &new_node)
{
	SpaceTree temp_tree;
	dGeomID temp_geom;
	bool not_found = true;
	int k, i;

	// check to see whether new_node is already part of tree top
	not_found = true;
	temp_geom = new_node.geom_dat->geom_id;
	for(k=0; k<top_tree.size() && not_found; k++)
		not_found = top_tree[k].geom_dat->geom_id != temp_geom;

	sub_tree.push_back(new_node);
	if(not_found)
	{
		if(dGeomIsSpace (temp_geom))
		{
			dSpaceID space_id = (dSpaceID)temp_geom;
			for(k=0; k<dSpaceGetNumGeoms (space_id); k++)
			{
				temp_geom = dSpaceGetGeom (space_id, k);

				// find this geom in the geom_array
				not_found = true;
				for(i=0; i<geom_array.size() && not_found; i++)
					not_found = temp_geom != geom_array[i].geom_id;
				assert(!not_found);

				temp_tree.geom_dat = &geom_array[--i];
				geom_array[i].marked  = false;
				recursive_fill_tree(top_tree, sub_tree.back().sub_tree, temp_tree);
			}
		}
	}
	else
	{
		top_tree.erase(&top_tree[k-1]);
	}
}

void recursive_print(const SpaceTree &tree, FILE *fp, const int& indent=0)
{
	dGeomID cur_geom;
	dVector4 dim;
	const dReal *pos;
	int k;
	const char *txt_ptr;

	for(k=0; k<indent; k++)
		fprintf(fp, "\t");

	cur_geom = tree.geom_dat->geom_id;
	k = tree.geom_dat->num;
	txt_ptr = tree.geom_dat->text;

	switch(dGeomGetClass (cur_geom))
	{
	case dSphereClass:
		pos = dGeomGetPosition(cur_geom);
		dim[0] = dGeomSphereGetRadius (cur_geom);
		fprintf(fp, "Sphere_%i [%s] pos(%f, %f, %f), r=%f\n", k, txt_ptr, pos[0], 
pos[1], pos[2], dim[0]);
		break;
	case dBoxClass:
		pos = dGeomGetPosition(cur_geom);
		dGeomBoxGetLengths (cur_geom, dim);
		fprintf(fp, "Box_%i [%s] pos(%f, %f, %f), sides(%f, %f, %f)\n", k, 
txt_ptr, pos[0], pos[1], pos[2], dim[0], dim[1], dim[2]);
		break;
	case dCCylinderClass:
		pos = dGeomGetPosition(cur_geom);
		dGeomCCylinderGetParams (cur_geom, dim, &dim[1]);
		fprintf(fp, "CCylinder_%i [%s] pos(%f, %f, %f), r=%f, l=%f\n", k, txt_ptr, 
pos[0], pos[1], pos[2], dim[0], dim[1]);
		break;
	case dRayClass:
		pos = dGeomGetPosition(cur_geom);
		dim[0] = dGeomRayGetLength (cur_geom);
		fprintf(fp, "Ray_%i [%s] pos(%f, %f, %f), l=%f\n", k, txt_ptr, pos[0], 
pos[1], pos[2], dim[0]);
		break;
	case dPlaneClass:
		dGeomPlaneGetParams (cur_geom, dim);
		fprintf(fp, "Plane_%i [%s] a=%f, b=%f, c=%f, d=%f\n", k, txt_ptr, dim[0], 
dim[1], dim[2], dim[3]);
		break;
	case dSimpleSpaceClass:
		fprintf(fp, "SimpleSpace_%i [%s]\n", k, txt_ptr);
		for(k=0; k<tree.sub_tree.size(); k++)
			recursive_print(tree.sub_tree[k], fp, indent+1);
		break;
	case dHashSpaceClass:
		fprintf(fp, "HashSpace_%i [%s]\n", k, txt_ptr);
		for(k=0; k<tree.sub_tree.size(); k++)
			recursive_print(tree.sub_tree[k], fp, indent+1);
		break;
	}
}

void write_geoms(FILE *fp)
{
	vector<SpaceTree> trees;
	SpaceTree temp_tree;
	int i;
	for(i=0; i<geom_array.size(); i++)
		geom_array[i].marked = true;

	for(i=0; i<geom_array.size(); i++)
	{
		if(geom_array[i].marked)
		{
			temp_tree.geom_dat = &geom_array[i];
			recursive_fill_tree(trees, trees, temp_tree);
		}
	}

	for(i=0; i<trees.size(); i++)
	{
		recursive_print(trees[i], fp);
	}
}

#else // ******** GEOM TRACKER NOT USED *******************************

void dSpaceDestroy2 (dxSpace *space)
{
	dSpaceDestroy(space);
}

void dGeomDestroy2 (dGeomID geom)
{
	dGeomDestroy(geom);
}

dGeomID dCreateSphere (dSpaceID space, dReal radius, char *buff)
{
	return dCreateSphere(space, radius);
}

dGeomID dCreateBox (dSpaceID space, dReal lx, dReal ly, dReal lz, char 
*buff)
{
	return dCreateBox(space, lx, ly, lz);
}

dGeomID dCreateCCylinder (dSpaceID space, dReal radius, dReal length, char 
*buff)
{
	return dCreateCCylinder(space, radius, length);
}

dGeomID dCreatePlane (dSpaceID space, dReal a, dReal b, dReal c, dReal d, 
char *buff)
{
	return dCreatePlane(space, a, b, c, d);
}

dGeomID dCreateRay (dSpaceID space, dReal length, char *buff)
{
	return dCreateRay(space, length);
}

dxSpace *dHashSpaceCreate (dxSpace *space, char *buff)
{
	return dHashSpaceCreate(space);
}

dxSpace *dSimpleSpaceCreate (dxSpace *space, char *buff)
{
	return dSimpleSpaceCreate(space);
}

void write_geoms(FILE *fp)
{
}

#endif

------=_NextPart_000_766_5989_6d8c--