Detour: Moved out commonly used code to DetourCommon.h/cpp

Detour: Moved pathfinder priority queue and node pool to DetourNode.h/cpp

Detour/Include/DetourCommon.h

@@ -0,0 +1,125 @@
+#ifndef DETOURCOMMON_H
+#define DETOURCOMMON_H
+
+//////////////////////////////////////////////////////////////////////////////////////////
+
+template<class T> inline void swap(T& a, T& b) { T t = a; a = b; b = t; }
+template<class T> inline T min(T a, T b) { return a < b ? a : b; }
+template<class T> inline T max(T a, T b) { return a > b ? a : b; }
+template<class T> inline T abs(T a) { return a < 0 ? -a : a; }
+template<class T> inline T sqr(T a) { return a*a; }
+template<class T> inline T clamp(T v, T mn, T mx) { return v < mn ? mn : (v > mx ? mx : v); }
+
+inline void vcross(float* dest, const float* v1, const float* v2)
+{
+	dest[0] = v1[1]*v2[2] - v1[2]*v2[1];
+	dest[1] = v1[2]*v2[0] - v1[0]*v2[2];
+	dest[2] = v1[0]*v2[1] - v1[1]*v2[0]; 
+}
+
+inline float vdot(const float* v1, const float* v2)
+{
+	return v1[0]*v2[0] + v1[1]*v2[1] + v1[2]*v2[2];
+}
+
+inline void vsub(float* dest, const float* v1, const float* v2)
+{
+	dest[0] = v1[0]-v2[0];
+	dest[1] = v1[1]-v2[1];
+	dest[2] = v1[2]-v2[2];
+}
+
+inline void vmin(float* mn, const float* v)
+{
+	mn[0] = min(mn[0], v[0]);
+	mn[1] = min(mn[1], v[1]);
+	mn[2] = min(mn[2], v[2]);
+}
+
+inline void vmax(float* mx, const float* v)
+{
+	mx[0] = max(mx[0], v[0]);
+	mx[1] = max(mx[1], v[1]);
+	mx[2] = max(mx[2], v[2]);
+}
+
+inline void vcopy(float* dest, const float* a)
+{
+	dest[0] = a[0];
+	dest[1] = a[1];
+	dest[2] = a[2];
+}
+
+inline float vdistSqr(const float* v1, const float* v2)
+{
+	float dx = v2[0] - v1[0];
+	float dy = v2[1] - v1[1];
+	float dz = v2[2] - v1[2];
+	return dx*dx + dy*dy + dz*dz;
+}
+
+inline void vnormalize(float* v)
+{
+	float d = 1.0f / sqrtf(sqr(v[0]) + sqr(v[1]) + sqr(v[2]));
+	v[0] *= d;
+	v[1] *= d;
+	v[2] *= d;
+}
+
+inline bool vequal(const float* p0, const float* p1)
+{
+	static const float thr = sqr(1.0f/16384.0f);
+	const float d = vdistSqr(p0, p1);
+	return d < thr;
+}
+
+inline int nextPow2(int v)
+{
+	v--;
+	v |= v >> 1;
+	v |= v >> 2;
+	v |= v >> 4;
+	v |= v >> 8;
+	v |= v >> 16;
+	v++;
+	return v;
+}
+
+inline float vdot2D(const float* u, const float* v)
+{
+	return u[0]*v[0] + u[2]*v[2];
+}
+
+inline float vperp2D(const float* u, const float* v)
+{
+	return u[2]*v[0] - u[0]*v[2];
+}
+
+inline float triArea2D(const float* a, const float* b, const float* c)
+{
+	return ((b[0]*a[2] - a[0]*b[2]) + (c[0]*b[2] - b[0]*c[2]) + (a[0]*c[2] - c[0]*a[2])) * 0.5f;
+}
+
+inline bool checkOverlapBox(const unsigned short amin[3], const unsigned short amax[3],
+							const unsigned short bmin[3], const unsigned short bmax[3])
+{
+	bool overlap = true;
+	overlap = (amin[0] > bmax[0] || amax[0] < bmin[0]) ? false : overlap;
+	overlap = (amin[1] > bmax[1] || amax[1] < bmin[1]) ? false : overlap;
+	overlap = (amin[2] > bmax[2] || amax[2] < bmin[2]) ? false : overlap;
+	return overlap;
+}
+
+void closestPtPointTriangle(float* closest, const float* p,
+							const float* a, const float* b, const float* c);
+
+bool intersectSegmentPoly2D(const float* p0, const float* p1,
+							const float* verts, int nverts,
+							float& tmin, float& tmax,
+							int& segMin, int& segMax);
+
+float distancePtSegSqr2D(const float* pt, const float* p, const float* q, float& t);
+
+void calcPolyCenter(float* tc, const unsigned short* idx, int nidx, const float* verts);
+
+#endif // DETOURCOMMON_H

Detour/Include/DetourNode.h

@@ -0,0 +1,149 @@
+//
+// Copyright (c) 2009 Mikko Mononen memon@inside.org
+//
+// This software is provided 'as-is', without any express or implied
+// warranty.  In no event will the authors be held liable for any damages
+// arising from the use of this software.
+// Permission is granted to anyone to use this software for any purpose,
+// including commercial applications, and to alter it and redistribute it
+// freely, subject to the following restrictions:
+// 1. The origin of this software must not be misrepresented; you must not
+//    claim that you wrote the original software. If you use this software
+//    in a product, an acknowledgment in the product documentation would be
+//    appreciated but is not required.
+// 2. Altered source versions must be plainly marked as such, and must not be
+//    misrepresented as being the original software.
+// 3. This notice may not be removed or altered from any source distribution.
+//
+
+#ifndef DETOURNODE_H
+#define DETOURNODE_H
+
+enum dtNodeFlags
+{
+	DT_NODE_OPEN = 0x01,
+	DT_NODE_CLOSED = 0x02,
+};
+
+struct dtNode
+{
+	float cost;
+	float total;
+	unsigned int id;
+	unsigned int pidx : 30;
+	unsigned int flags : 2;
+};
+
+class dtNodePool
+{
+public:
+	dtNodePool(int maxNodes, int hashSize);
+	~dtNodePool();
+	inline void operator=(const dtNodePool&) {}
+	void clear();
+	dtNode* getNode(unsigned int id);
+	const dtNode* findNode(unsigned int id) const;
+
+	inline unsigned int getNodeIdx(const dtNode* node) const
+	{
+		if (!node) return 0;
+		return (unsigned int)(node - m_nodes)+1;
+	}
+
+	inline dtNode* getNodeAtIdx(unsigned int idx)
+	{
+		if (!idx) return 0;
+		return &m_nodes[idx-1];
+	}
+	
+	inline int getMemUsed() const
+	{
+		return sizeof(*this) +
+		sizeof(dtNode)*m_maxNodes +
+		sizeof(unsigned short)*m_maxNodes +
+		sizeof(unsigned short)*m_hashSize;
+	}
+	
+private:
+	inline unsigned int hashint(unsigned int a) const
+	{
+		a += ~(a<<15);
+		a ^=  (a>>10);
+		a +=  (a<<3);
+		a ^=  (a>>6);
+		a += ~(a<<11);
+		a ^=  (a>>16);
+		return a;
+	}
+	
+	dtNode* m_nodes;
+	unsigned short* m_first;
+	unsigned short* m_next;
+	const int m_maxNodes;
+	const int m_hashSize;
+	int m_nodeCount;
+};
+
+class dtNodeQueue
+{
+public:
+	dtNodeQueue(int n);
+	~dtNodeQueue();
+	inline void operator=(dtNodeQueue&) {}
+	
+	inline void clear()
+	{
+		m_size = 0;
+	}
+	
+	inline dtNode* top()
+	{
+		return m_heap[0];
+	}
+	
+	inline dtNode* pop()
+	{
+		dtNode* result = m_heap[0];
+		m_size--;
+		trickleDown(0, m_heap[m_size]);
+		return result;
+	}
+	
+	inline void push(dtNode* node)
+	{
+		m_size++;
+		bubbleUp(m_size-1, node);
+	}
+	
+	inline void modify(dtNode* node)
+	{
+		for (int i = 0; i < m_size; ++i)
+		{
+			if (m_heap[i] == node)
+			{
+				bubbleUp(i, node);
+				return;
+			}
+		}
+	}
+	
+	inline bool empty() const { return m_size == 0; }
+	
+	inline int getMemUsed() const
+	{
+		return sizeof(*this) +
+		sizeof(dtNode*)*(m_capacity+1);
+	}
+	
+	
+private:
+	void bubbleUp(int i, dtNode* node);
+	void trickleDown(int i, dtNode* node);
+	
+	dtNode** m_heap;
+	const int m_capacity;
+	int m_size;
+};		
+
+
+#endif // DETOURNODE_H

Detour/Source/DetourCommon.cpp

@@ -0,0 +1,188 @@
+
+#include <math.h>
+#include "DetourCommon.h"
+
+void closestPtPointTriangle(float* closest, const float* p,
+							const float* a, const float* b, const float* c)
+{
+	// Check if P in vertex region outside A
+	float ab[3], ac[3], ap[3];
+	vsub(ab, b, a);
+	vsub(ac, c, a);
+	vsub(ap, p, a);
+	float d1 = vdot(ab, ap);
+	float d2 = vdot(ac, ap);
+	if (d1 <= 0.0f && d2 <= 0.0f)
+	{
+		// barycentric coordinates (1,0,0)
+		vcopy(closest, a);
+		return;
+	}
+	
+	// Check if P in vertex region outside B
+	float bp[3];
+	vsub(bp, p, b);
+	float d3 = vdot(ab, bp);
+	float d4 = vdot(ac, bp);
+	if (d3 >= 0.0f && d4 <= d3)
+	{
+		// barycentric coordinates (0,1,0)
+		vcopy(closest, b);
+		return;
+	}
+	
+	// Check if P in edge region of AB, if so return projection of P onto AB
+	float vc = d1*d4 - d3*d2;
+	if (vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f)
+	{
+		// barycentric coordinates (1-v,v,0)
+		float v = d1 / (d1 - d3);
+		closest[0] = a[0] + v * ab[0];
+		closest[1] = a[1] + v * ab[1];
+		closest[2] = a[2] + v * ab[2];
+		return;
+	}
+	
+	// Check if P in vertex region outside C
+	float cp[3];
+	vsub(cp, p, c);
+	float d5 = vdot(ab, cp);
+	float d6 = vdot(ac, cp);
+	if (d6 >= 0.0f && d5 <= d6)
+	{
+		// barycentric coordinates (0,0,1)
+		vcopy(closest, c);
+		return;
+	}
+	
+	// Check if P in edge region of AC, if so return projection of P onto AC
+	float vb = d5*d2 - d1*d6;
+	if (vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f)
+	{
+		// barycentric coordinates (1-w,0,w)
+		float w = d2 / (d2 - d6);
+		closest[0] = a[0] + w * ac[0];
+		closest[1] = a[1] + w * ac[1];
+		closest[2] = a[2] + w * ac[2];
+		return;
+	}
+	
+	// Check if P in edge region of BC, if so return projection of P onto BC
+	float va = d3*d6 - d5*d4;
+	if (va <= 0.0f && (d4 - d3) >= 0.0f && (d5 - d6) >= 0.0f)
+	{
+		// barycentric coordinates (0,1-w,w)
+		float w = (d4 - d3) / ((d4 - d3) + (d5 - d6));
+		closest[0] = b[0] + w * (c[0] - b[0]);
+		closest[1] = b[1] + w * (c[1] - b[1]);
+		closest[2] = b[2] + w * (c[2] - b[2]);
+		return;
+	}
+	
+	// P inside face region. Compute Q through its barycentric coordinates (u,v,w)
+	float denom = 1.0f / (va + vb + vc);
+	float v = vb * denom;
+	float w = vc * denom;
+	closest[0] = a[0] + ab[0] * v + ac[0] * w;
+	closest[1] = a[1] + ab[1] * v + ac[1] * w;
+	closest[2] = a[2] + ab[2] * v + ac[2] * w;
+}
+
+bool intersectSegmentPoly2D(const float* p0, const float* p1,
+							const float* verts, int nverts,
+							float& tmin, float& tmax,
+							int& segMin, int& segMax)
+{
+	static const float EPS = 0.00000001f;
+	
+	tmin = 0;
+	tmax = 1;
+	segMin = -1;
+	segMax = -1;
+	
+	float dir[3];
+	vsub(dir, p1, p0);
+	
+	for (int i = 0, j = nverts-1; i < nverts; j=i++)
+	{
+		float edge[3], diff[3];
+		vsub(edge, &verts[i*3], &verts[j*3]);
+		vsub(diff, p0, &verts[j*3]);
+		float n = vperp2D(edge, diff);
+		float d = -vperp2D(edge, dir);
+		if (fabs(d) < EPS)
+		{
+			// S is nearly parallel to this edge
+			if (n < 0)
+				return false;
+			else
+				continue;
+		}
+		float t = n / d;
+		if (d < 0)
+		{
+			// segment S is entering across this edge
+			if (t > tmin)
+			{
+				tmin = t;
+				segMin = j;
+				// S enters after leaving polygon
+				if (tmin > tmax)
+					return false;
+			}
+		}
+		else
+		{
+			// segment S is leaving across this edge
+			if (t < tmax)
+			{
+				tmax = t;
+				segMax = j;
+				// S leaves before entering polygon
+				if (tmax < tmin)
+					return false;
+			}
+		}
+	}
+	
+	return true;
+}
+
+float distancePtSegSqr2D(const float* pt, const float* p, const float* q, float& t)
+{
+	float pqx = q[0] - p[0];
+	float pqz = q[2] - p[2];
+	float dx = pt[0] - p[0];
+	float dz = pt[2] - p[2];
+	float d = pqx*pqx + pqz*pqz;
+	t = pqx*dx + pqz*dz;
+	if (d > 0)
+		t /= d;
+	if (t < 0)
+		t = 0;
+	else if (t > 1)
+		t = 1;
+	
+	dx = p[0] + t*pqx - pt[0];
+	dz = p[2] + t*pqz - pt[2];
+	
+	return dx*dx + dz*dz;
+}
+
+void calcPolyCenter(float* tc, const unsigned short* idx, int nidx, const float* verts)
+{
+	tc[0] = 0.0f;
+	tc[1] = 0.0f;
+	tc[2] = 0.0f;
+	for (int j = 0; j < nidx; ++j)
+	{
+		const float* v = &verts[idx[j]*3];
+		tc[0] += v[0];
+		tc[1] += v[1];
+		tc[2] += v[2];
+	}
+	const float s = 1.0f / nidx;
+	tc[0] *= s;
+	tc[1] *= s;
+	tc[2] *= s;
+}

Detour/Source/DetourNode.cpp

@@ -0,0 +1,121 @@
+
+#include "DetourNode.h"
+#include <string.h>
+
+//////////////////////////////////////////////////////////////////////////////////////////
+dtNodePool::dtNodePool(int maxNodes, int hashSize) :
+	m_maxNodes(maxNodes),
+	m_hashSize(hashSize),
+	m_nodes(0),
+	m_first(0),
+	m_next(0)
+{
+	m_nodes = new dtNode[m_maxNodes];
+	m_next = new unsigned short[m_maxNodes];
+	m_first = new unsigned short[hashSize];
+	memset(m_first, 0xff, sizeof(unsigned short)*m_hashSize);
+	memset(m_next, 0xff, sizeof(unsigned short)*m_maxNodes);
+}
+
+dtNodePool::~dtNodePool()
+{
+	delete [] m_nodes;
+	delete [] m_next;
+	delete [] m_first;
+}
+
+void dtNodePool::clear()
+{
+	memset(m_first, 0xff, sizeof(unsigned short)*m_hashSize);
+	m_nodeCount = 0;
+}
+
+const dtNode* dtNodePool::findNode(unsigned int id) const
+{
+	unsigned int bucket = hashint(id) & (m_hashSize-1);
+	unsigned short i = m_first[bucket];
+	while (i != 0xffff)
+	{
+		if (m_nodes[i].id == id)
+			return &m_nodes[i];
+		i = m_next[i];
+	}
+	return 0;
+}
+
+dtNode* dtNodePool::getNode(unsigned int id)
+{
+	unsigned int bucket = hashint(id) & (m_hashSize-1);
+	unsigned short i = m_first[bucket];
+	dtNode* node = 0;
+	while (i != 0xffff)
+	{
+		if (m_nodes[i].id == id)
+			return &m_nodes[i];
+		i = m_next[i];
+	}
+	
+	if (m_nodeCount >= m_maxNodes)
+		return 0;
+	
+	i = (unsigned short)m_nodeCount;
+	m_nodeCount++;
+	
+	// Init node
+	node = &m_nodes[i];
+	node->pidx = 0;
+	node->cost = 0;
+	node->total = 0;
+	node->id = id;
+	node->flags = 0;
+	
+	m_next[i] = m_first[bucket];
+	m_first[bucket] = i;
+	
+	return node;
+}
+
+
+//////////////////////////////////////////////////////////////////////////////////////////
+dtNodeQueue::dtNodeQueue(int n) :
+	m_capacity(n),
+	m_size(0),
+	m_heap(0)
+{
+	m_heap = new dtNode*[m_capacity+1];
+}
+
+dtNodeQueue::~dtNodeQueue()
+{
+	delete [] m_heap;
+}
+
+void dtNodeQueue::bubbleUp(int i, dtNode* node)
+{
+	int parent = (i-1)/2;
+	// note: (index > 0) means there is a parent
+	while ((i > 0) && (m_heap[parent]->total > node->total))
+	{
+		m_heap[i] = m_heap[parent];
+		i = parent;
+		parent = (i-1)/2;
+	}
+	m_heap[i] = node;
+}
+
+void dtNodeQueue::trickleDown(int i, dtNode* node)
+{
+	int child = (i*2)+1;
+	while (child < m_size)
+	{
+		if (((child+1) < m_size) && 
+			(m_heap[child]->total > m_heap[child+1]->total))
+		{
+			child++;
+		}
+		m_heap[i] = m_heap[child];
+		i = child;
+		child = (i*2)+1;
+	}
+	bubbleUp(i, node);
+}