Initial commit

engines/hpl1/engine/libraries/newton/physics/dgBroadPhaseCollision.h

@@ -0,0 +1,220 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+ *
+ * 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 __AFX_BROADPHASE__INCLUDED__
+#define __AFX_BROADPHASE__INCLUDED__
+
+#include "dgBody.h"
+#include "dgContact.h"
+#include "hpl1/engine/libraries/newton/core/dg.h"
+
+
+//#define DG_OCTREE_MAX_DEPTH       1
+//#define DG_OCTREE_MAX_DEPTH       6
+#define DG_OCTREE_MAX_DEPTH 7
+
+typedef void(dgApi *OnBodiesInAABB)(dgBody *body, void *const userData);
+typedef void(dgApi *OnLeavingWorldAction)(dgBody *body, dgInt32 threadIndex);
+
+class dgCellPair {
+public:
+	dgBroadPhaseCell *m_cell_A;
+	dgBroadPhaseCell *m_cell_B;
+};
+
+class dgSortArrayEntry {
+public:
+	dgBody *m_body;
+	dgFloat32 m_key;
+};
+
+class dgSortArray : public dgList<dgSortArrayEntry> {
+public:
+	dgSortArray();
+	~dgSortArray();
+
+	void Add(dgBody *const body);
+	void Remove(dgBody *const body);
+	dgFloat32 Sort();
+	void InvalidateCache();
+
+	dgFloat32 RayCast(dgFloat32 minT, const dgLineBox &line, OnRayCastAction filter, OnRayPrecastAction prefilter, void *const userData) const;
+	bool SanityCheck();
+
+	dgInt8 m_index;
+	dgInt8 m_isSorted;
+};
+
+class dgBroadPhaseCell {
+public:
+	dgBroadPhaseCell();
+	~dgBroadPhaseCell();
+
+private:
+	void Init(dgInt32 layer, dgMemoryAllocator *allocator);
+	void Add(dgBody *const body);
+	void Remove(dgBody *const body);
+
+	void Sort();
+	void UpdateAutoPair(dgWorld *const world, dgInt32 threadIndex);
+
+	dgSortArray m_sort[3];
+	dgSortArray *m_lastSortArray;
+
+	dgInt32 m_count;
+	dgInt8 m_active;
+	dgInt8 m_layerIndex;
+
+	friend class dgBody;
+	friend class dgBroadPhaseLayer;
+	friend class dgBroadPhaseCollision;
+	friend class dgBroadPhaseCellPairsWorkerThread;
+};
+
+class dgBroadPhaseLayer : public dgTree<dgBroadPhaseCell, dgUnsigned32> {
+public:
+	dgBroadPhaseLayer();
+	~dgBroadPhaseLayer();
+
+private:
+	void Init(dgWorld *const world, dgFloat32 cellSize, dgInt32 layerIndex);
+
+	inline dgUnsigned32 GetKey(dgInt32 x, dgInt32 z) const {
+		dgUnsigned32 key = dgUnsigned32((z << DG_OCTREE_MAX_DEPTH) + x);
+		return key;
+	}
+
+	inline void KeyToIndex(dgInt32 key, dgInt32 &x, dgInt32 &z) const {
+		x = key & ((1 << DG_OCTREE_MAX_DEPTH) - 1);
+		z = key >> DG_OCTREE_MAX_DEPTH;
+	}
+
+	inline dgBroadPhaseCell *Find(dgInt32 x, dgInt32 z) const {
+		dgTreeNode *const node = dgTree<dgBroadPhaseCell, dgUnsigned32>::Find(GetKey(x, z));
+		return node ? &node->GetInfo() : NULL;
+	}
+	dgBroadPhaseCell *FindCreate(dgInt32 x, dgInt32 z);
+
+	dgWorld *m_me;
+	dgFloat32 m_cellSize;
+	dgFloat32 m_invCellSize;
+	dgInt16 m_layerIndex;
+
+	friend class dgBroadPhaseCollision;
+};
+
+class dgBroadPhaseApplyExternalForce : public dgWorkerThread {
+public:
+	virtual void ThreadExecute();
+
+	dgInt32 m_step;
+	dgInt32 m_count;
+	dgInt32 m_skipForceUpdate;
+	dgFloat32 m_timeStep;
+	dgWorld *m_world;
+	dgBody **m_bodies;
+};
+
+class dgBroadPhaseCellPairsWorkerThread : public dgWorkerThread {
+public:
+	virtual void ThreadExecute();
+
+	dgInt32 m_step;
+	dgInt32 m_count;
+	dgWorld *m_world;
+	dgCellPair *m_pairs;
+};
+
+class dgBroadPhaseCalculateContactsWorkerThread : public dgWorkerThread {
+public:
+	void Realloc(dgInt32 jointsCount, dgInt32 contactCount, dgInt32 threadIndex);
+	virtual void ThreadExecute();
+
+	dgInt32 m_step;
+	dgInt32 m_count;
+	dgInt32 m_useSimd;
+	dgFloat32 m_timestep;
+	dgWorld *m_world;
+};
+
+class dgBroadPhaseMaterialCallbackWorkerThread : public dgWorkerThread {
+public:
+	virtual void ThreadExecute();
+
+	dgInt32 m_step;
+	dgInt32 m_count;
+	dgInt32 m_useSimd;
+	dgFloat32 m_timestep;
+	dgWorld *m_world;
+	dgCollidingPairCollector::dgPair *m_pairs;
+};
+
+class dgBroadPhaseCollision {
+public:
+	void GetWorldSize(dgVector &p0, dgVector &p1) const;
+	void SetWorldSize(const dgVector &min, const dgVector &max);
+	void RayCast(const dgVector &p0, const dgVector &p1, OnRayCastAction filter, OnRayPrecastAction prefilter, void *const userData);
+	dgInt32 ConvexCast(dgCollision *const shape, const dgMatrix &p0, const dgVector &p1, dgFloat32 &timetoImpact, OnRayPrecastAction prefilter, void *const userData, dgConvexCastReturnInfo *const info, dgInt32 maxContacts, dgInt32 threadIndex);
+	void ForEachBodyInAABB(const dgVector &q0, const dgVector &q1, OnBodiesInAABB callback, void *const userData) const;
+
+private:
+	dgBroadPhaseCollision(dgMemoryAllocator *allocator);
+	~dgBroadPhaseCollision();
+
+	void Init();
+	void Add(dgBody *const body);
+	void Remove(dgBody *const body);
+	void InvalidateCache();
+
+	dgUnsigned32 UpdateContactsBroadPhaseBegin(dgFloat32 tiemstep, bool collisioUpdate, dgUnsigned32 ticks);
+	void UpdateContactsBroadPhaseEnd(dgFloat32 tiemstep);
+
+	void UpdateContacts(dgFloat32 tiemstep, bool collisioUpdate);
+	void UpdateContactsSimd(dgFloat32 tiemstep, bool collisioUpdate);
+
+	void UpdateBodyBroadphase(dgBody *const body, dgInt32 threadIndex);
+
+	void UpdatePairs(dgBroadPhaseCell &cellA, dgBroadPhaseCell &cellB, dgInt32 threadIndex);
+	void UpdatePairs(dgBody *const body0, dgSortArray::dgListNode *const listNode, dgInt32 axisX, dgInt32 threadIndex);
+
+	dgVector m_min;
+	dgVector m_max;
+	dgVector m_appMinBox;
+	dgVector m_appMaxBox;
+
+	dgVector m_boxSize;
+	dgBroadPhaseCell m_inactiveList;
+	dgBroadPhaseLayer m_layerMap[DG_OCTREE_MAX_DEPTH];
+	dgBroadPhaseApplyExternalForce m_applyExtForces[DG_MAXIMUN_THREADS];
+	dgBroadPhaseCellPairsWorkerThread m_cellPairsWorkerThreads[DG_MAXIMUN_THREADS];
+	dgBroadPhaseMaterialCallbackWorkerThread m_materialCallbackWorkerThreads[DG_MAXIMUN_THREADS];
+	dgBroadPhaseCalculateContactsWorkerThread m_calculateContactsWorkerThreads[DG_MAXIMUN_THREADS];
+
+	//  static void ForceAndtorque (void** const m_userParamArray, dgInt32 threadID);
+	//  dgWorld* m_me;
+	dgFloat32 m_worlSize;
+
+	friend class dgBody;
+	friend class dgWorld;
+	friend class dgBroadPhaseCellPairsWorkerThread;
+};
+
+#endif