252 lines
7.9 KiB
C++
252 lines
7.9 KiB
C++
/* Copyright (c) <2003-2019> <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 __DG_AABB_POLYGON_SOUP_H_
|
|
#define __DG_AABB_POLYGON_SOUP_H_
|
|
|
|
#include "dgStdafx.h"
|
|
#include "dgIntersections.h"
|
|
#include "dgPolygonSoupDatabase.h"
|
|
|
|
|
|
class dgPolygonSoupDatabaseBuilder;
|
|
|
|
|
|
class dgAABBPolygonSoup: public dgPolygonSoupDatabase
|
|
{
|
|
public:
|
|
class dgNode
|
|
{
|
|
public:
|
|
enum dgNodeType
|
|
{
|
|
m_binary = 0,
|
|
m_leaf,
|
|
};
|
|
|
|
class dgLeafNodePtr
|
|
{
|
|
#define DG_INDEX_COUNT_BITS 6
|
|
|
|
public:
|
|
DG_INLINE dgLeafNodePtr ()
|
|
{
|
|
dgAssert (0);
|
|
}
|
|
|
|
DG_INLINE dgLeafNodePtr (dgUnsigned32 node)
|
|
{
|
|
m_node = node;
|
|
dgAssert (!IsLeaf());
|
|
}
|
|
|
|
DG_INLINE dgUnsigned32 IsLeaf () const
|
|
{
|
|
return m_node & 0x80000000;
|
|
}
|
|
|
|
DG_INLINE dgUnsigned32 GetCount() const
|
|
{
|
|
dgAssert (IsLeaf());
|
|
return (m_node & (~0x80000000)) >> (32 - DG_INDEX_COUNT_BITS - 1);
|
|
}
|
|
|
|
DG_INLINE dgUnsigned32 GetIndex() const
|
|
{
|
|
dgAssert (IsLeaf());
|
|
return m_node & (~(-(1 << (32 - DG_INDEX_COUNT_BITS - 1))));
|
|
}
|
|
|
|
|
|
DG_INLINE dgLeafNodePtr (dgUnsigned32 faceIndexCount, dgUnsigned32 faceIndexStart)
|
|
{
|
|
dgAssert (faceIndexCount < (1<<DG_INDEX_COUNT_BITS));
|
|
m_node = 0x80000000 | (faceIndexCount << (32 - DG_INDEX_COUNT_BITS - 1)) | faceIndexStart;
|
|
}
|
|
|
|
DG_INLINE dgNode* GetNode (const void* const root) const
|
|
{
|
|
return ((dgNode*) root) + m_node;
|
|
}
|
|
|
|
union {
|
|
dgUnsigned32 m_node;
|
|
};
|
|
};
|
|
|
|
|
|
dgNode ()
|
|
:m_indexBox0(0)
|
|
,m_indexBox1(0)
|
|
,m_left(0)
|
|
,m_right(0)
|
|
{
|
|
}
|
|
|
|
DG_INLINE dgFloat32 RayDistance (const dgFastRayTest& ray, const dgTriplex* const vertexArray) const
|
|
{
|
|
dgVector minBox (&vertexArray[m_indexBox0].m_x);
|
|
dgVector maxBox (&vertexArray[m_indexBox1].m_x);
|
|
minBox = minBox & dgVector::m_triplexMask;
|
|
maxBox = maxBox & dgVector::m_triplexMask;
|
|
return ray.BoxIntersect(minBox, maxBox);
|
|
}
|
|
|
|
DG_INLINE dgFloat32 BoxPenetration (const dgFastAABBInfo& obb, const dgTriplex* const vertexArray) const
|
|
{
|
|
dgVector p0 (&vertexArray[m_indexBox0].m_x);
|
|
dgVector p1 (&vertexArray[m_indexBox1].m_x);
|
|
p0 = p0 & dgVector::m_triplexMask;
|
|
p1 = p1 & dgVector::m_triplexMask;
|
|
dgVector minBox (p0 - obb.m_p1);
|
|
dgVector maxBox (p1 - obb.m_p0);
|
|
dgAssert(maxBox.m_x >= minBox.m_x);
|
|
dgAssert(maxBox.m_y >= minBox.m_y);
|
|
dgAssert(maxBox.m_z >= minBox.m_z);
|
|
|
|
dgVector mask ((minBox * maxBox) < dgVector::m_zero);
|
|
dgVector dist (maxBox.GetMin (minBox.Abs()) & mask);
|
|
dist = dist.GetMin(dist.ShiftTripleRight());
|
|
dist = dist.GetMin(dist.ShiftTripleRight());
|
|
|
|
if (dist.GetScalar() > dgFloat32 (0.0f)) {
|
|
dgVector origin (dgVector::m_half * (p1 + p0));
|
|
dgVector size (dgVector::m_half * (p1 - p0));
|
|
|
|
origin = obb.UntransformVector(origin);
|
|
size = obb.m_absDir.RotateVector(size);
|
|
dgVector q0 (origin - size);
|
|
dgVector q1 (origin + size);
|
|
dgVector minBox1 (q0 - obb.m_size);
|
|
dgVector maxBox1 (q1 + obb.m_size);
|
|
dgAssert(maxBox1.m_x >= minBox1.m_x);
|
|
dgAssert(maxBox1.m_y >= minBox1.m_y);
|
|
dgAssert(maxBox1.m_z >= minBox1.m_z);
|
|
dgVector mask1 ((minBox1 * maxBox1) < dgVector::m_zero);
|
|
dgVector dist1 (maxBox1.GetMin (minBox1.Abs()) & mask1);
|
|
dist1 = dist1.GetMin(dist1.ShiftTripleRight());
|
|
dist1 = dist1.GetMin(dist1.ShiftTripleRight());
|
|
dist = dist.GetMin(dist1);
|
|
} else {
|
|
dgVector p1p0((minBox.Abs()).GetMin(maxBox.Abs()).AndNot(mask));
|
|
dist = p1p0.DotProduct(p1p0);
|
|
dist = dist.Sqrt() * dgVector::m_negOne;
|
|
}
|
|
return dist.GetScalar();
|
|
}
|
|
|
|
DG_INLINE dgFloat32 BoxIntersect (const dgFastRayTest& ray, const dgFastRayTest& obbRay, const dgFastAABBInfo& obb, const dgTriplex* const vertexArray) const
|
|
{
|
|
dgVector p0 (&vertexArray[m_indexBox0].m_x);
|
|
dgVector p1 (&vertexArray[m_indexBox1].m_x);
|
|
p0 = p0 & dgVector::m_triplexMask;
|
|
p1 = p1 & dgVector::m_triplexMask;
|
|
|
|
dgVector minBox (p0 - obb.m_p1);
|
|
dgVector maxBox (p1 - obb.m_p0);
|
|
dgFloat32 dist = ray.BoxIntersect(minBox, maxBox);
|
|
if (dist < dgFloat32 (1.0f)) {
|
|
dgVector origin (dgVector::m_half * (p1 + p0));
|
|
dgVector size (dgVector::m_half * (p1 - p0));
|
|
|
|
origin = obb.UntransformVector(origin);
|
|
size = obb.m_absDir.RotateVector(size);
|
|
dgVector q0 (origin - size);
|
|
dgVector q1 (origin + size);
|
|
|
|
dgVector minBox1 (q0 - obb.m_size);
|
|
dgVector maxBox1 (q1 + obb.m_size);
|
|
dgFloat32 dist1 = obbRay.BoxIntersect(minBox1, maxBox1);
|
|
dist = (dist1 > dgFloat32 (1.0f)) ? dist1 : dgMax (dist1, dist);
|
|
}
|
|
return dist;
|
|
}
|
|
|
|
dgInt32 m_indexBox0;
|
|
dgInt32 m_indexBox1;
|
|
dgLeafNodePtr m_left;
|
|
dgLeafNodePtr m_right;
|
|
};
|
|
|
|
class dgSpliteInfo;
|
|
class dgNodeBuilder;
|
|
|
|
virtual void GetAABB (dgVector& p0, dgVector& p1) const;
|
|
virtual void Serialize (dgSerialize callback, void* const userData) const;
|
|
virtual void Deserialize (dgDeserialize callback, void* const userData, dgInt32 revisionNumber);
|
|
|
|
protected:
|
|
dgAABBPolygonSoup ();
|
|
virtual ~dgAABBPolygonSoup ();
|
|
|
|
void Create (const dgPolygonSoupDatabaseBuilder& builder, bool optimizedBuild);
|
|
void CalculateAdjacendy ();
|
|
virtual void ForAllSectorsRayHit (const dgFastRayTest& ray, dgFloat32 maxT, dgRayIntersectCallback callback, void* const context) const;
|
|
virtual void ForAllSectors (const dgFastAABBInfo& obbAabb, const dgVector& boxDistanceTravel, dgFloat32 m_maxT, dgAABBIntersectCallback callback, void* const context) const;
|
|
|
|
|
|
DG_INLINE void* GetRootNode() const
|
|
{
|
|
return m_aabb;
|
|
}
|
|
|
|
DG_INLINE void* GetBackNode(const void* const root) const
|
|
{
|
|
dgNode* const node = (dgNode*) root;
|
|
return node->m_left.IsLeaf() ? NULL : node->m_left.GetNode(m_aabb);
|
|
}
|
|
|
|
DG_INLINE void* GetFrontNode(const void* const root) const
|
|
{
|
|
dgNode* const node = (dgNode*) root;
|
|
return node->m_right.IsLeaf() ? NULL : node->m_right.GetNode(m_aabb);
|
|
}
|
|
|
|
DG_INLINE void GetNodeAABB(const void* const root, dgVector& p0, dgVector& p1) const
|
|
{
|
|
const dgNode* const node = (dgNode*)root;
|
|
p0 = dgVector (&((dgTriplex*)m_localVertex)[node->m_indexBox0].m_x);
|
|
p1 = dgVector (&((dgTriplex*)m_localVertex)[node->m_indexBox1].m_x);
|
|
p0 = p0 & dgVector::m_triplexMask;
|
|
p1 = p1 & dgVector::m_triplexMask;
|
|
}
|
|
|
|
virtual dgVector ForAllSectorsSupportVectex (const dgVector& dir) const;
|
|
|
|
private:
|
|
dgNodeBuilder* BuildTopDown (dgNodeBuilder* const leafArray, dgInt32 firstBox, dgInt32 lastBox, dgNodeBuilder** const allocator) const;
|
|
dgFloat32 CalculateFaceMaxSize (const dgVector* const vertex, dgInt32 indexCount, const dgInt32* const indexArray) const;
|
|
// static dgIntersectStatus CalculateManifoldFaceEdgeNormals (void* const context, const dgFloat32* const polygon, dgInt32 strideInBytes, const dgInt32* const indexArray, dgInt32 indexCount);
|
|
static dgIntersectStatus CalculateDisjointedFaceEdgeNormals (void* const context, const dgFloat32* const polygon, dgInt32 strideInBytes, const dgInt32* const indexArray, dgInt32 indexCount, dgFloat32 hitDistance);
|
|
static dgIntersectStatus CalculateAllFaceEdgeNormals (void* const context, const dgFloat32* const polygon, dgInt32 strideInBytes, const dgInt32* const indexArray, dgInt32 indexCount, dgFloat32 hitDistance);
|
|
void ImproveNodeFitness (dgNodeBuilder* const node) const;
|
|
|
|
dgInt32 m_nodesCount;
|
|
dgInt32 m_indexCount;
|
|
dgNode* m_aabb;
|
|
dgInt32* m_indices;
|
|
};
|
|
|
|
|
|
#endif
|
|
|
|
|