AnotherFoxGuy/bounce
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

improve contact clustering algorithm

Browse Source
This commit is contained in:
Irlan 2018-03-23 00:12:59 -03:00
parent 3da2a25bd7
commit 5a80171744
5 changed files with 425 additions and 357 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

44
examples/testbed/tests/cluster_test.h
View File

@ -32,27 +32,28 @@ public:
g_camera.m_zoom = 10.0f; g_camera.m_zoom = 10.0f;
// Initialize observations // Initialize observations
b3StackArray<b3Observation, 256> tempObservations; for (u32 i = 0; i < 90; ++i)
tempObservations.Resize(90);
for (u32 i = 0; i < tempObservations.Count(); ++i)
{ {
float32 x = RandomFloat(-1.0f, 1.0f); float32 x = RandomFloat(-1.0f, 1.0f);
float32 y = RandomFloat(-1.0f, 1.0f); float32 y = RandomFloat(-1.0f, 1.0f);
float32 z = RandomFloat(-1.0f, 1.0f); float32 z = RandomFloat(-1.0f, 1.0f);
tempObservations[i].point.Set(x, y, z); b3Observation obs;
tempObservations[i].point = b3Normalize(tempObservations[i].point); obs.point.Set(x, y, z);
tempObservations[i].cluster = 0xFFFFFFFF; obs.point = b3Normalize(obs.point);
obs.cluster = B3_NULL_CLUSTER;
m_solver.AddObservation(obs);
} }
// Initialize clusters // Initialize clusters
b3StackArray<b3Cluster, 3> tempClusters; m_solver.Solve();
b3InitializeClusters(tempClusters, tempObservations);
// Clusterize const b3Array<b3Cluster>& clusters = m_solver.GetClusters();
b3Clusterize(m_clusters, m_observs, tempClusters, tempObservations);
m_colors.Resize(clusters.Count());
for (u32 i = 0; i < m_clusters.Count(); ++i)
for (u32 i = 0; i < clusters.Count(); ++i)
{ {
m_colors[i] = b3Color(RandomFloat(0.0f, 1.0f), RandomFloat(0.0f, 1.0f), RandomFloat(0.0f, 1.0f)); m_colors[i] = b3Color(RandomFloat(0.0f, 1.0f), RandomFloat(0.0f, 1.0f), RandomFloat(0.0f, 1.0f));
} }
@ -60,14 +61,17 @@ public:
void Step() void Step()
{ {
for (u32 i = 0; i < m_clusters.Count(); ++i) const b3Array<b3Observation>& observations = m_solver.GetObservations();
const b3Array<b3Cluster>& clusters = m_solver.GetClusters();
for (u32 i = 0; i < clusters.Count(); ++i)
{ {
g_debugDraw->DrawSegment(b3Vec3(0, 0, 0), m_clusters[i].centroid, b3Color(1, 1, 1)); g_debugDraw->DrawSegment(b3Vec3(0, 0, 0), clusters[i].centroid, b3Color(1, 1, 1));
g_debugDraw->DrawPoint(m_clusters[i].centroid, 4.0f, m_colors[i]); g_debugDraw->DrawPoint(clusters[i].centroid, 4.0f, m_colors[i]);
for (u32 j = 0; j < m_observs.Count(); ++j) for (u32 j = 0; j < observations.Count(); ++j)
{ {
b3Observation obs = m_observs[j]; b3Observation obs = observations[j];
if (obs.cluster == i) if (obs.cluster == i)
{ {
g_debugDraw->DrawPoint(obs.point, 4.0f, m_colors[i]); g_debugDraw->DrawPoint(obs.point, 4.0f, m_colors[i]);
@ -81,9 +85,9 @@ public:
return new Cluster(); return new Cluster();
} }
b3StackArray<b3Observation, 256> m_observs; b3ClusterSolver m_solver;
b3StackArray<b3Cluster, 3> m_clusters;
b3Color m_colors[3]; b3StackArray<b3Color, 32> m_colors;
}; };
#endif #endif

89
include/bounce/dynamics/contacts/contact_cluster.h
View File

@ -23,47 +23,92 @@
#include <bounce/common/template/array.h> #include <bounce/common/template/array.h>
#include <bounce/dynamics/contacts/manifold.h> #include <bounce/dynamics/contacts/manifold.h>
#define B3_NULL_CLUSTER (0xFFFFFFFF)
// Used for contact cluster reduction. // Used for contact cluster reduction.
struct b3ClusterVertex struct b3ClusterPolygonVertex
{ {
b3Vec3 position; // point on the cluster plane b3Vec3 position; // point on the cluster plane
u32 clipIndex; // where did this vertex came from (hint: local point) u32 clipIndex; // where did this vertex came from
}; };
// Used for contact cluster reduction. // Used for contact cluster reduction.
typedef b3Array<b3ClusterVertex> b3ClusterPolygon; typedef b3Array<b3ClusterPolygonVertex> b3ClusterPolygon;
// A observation represents a contact normal. // Weld a convex polygon such that the polygon normal points to a given direction.
void b3WeldPolygon(b3ClusterPolygon& pOut,
const b3ClusterPolygon& pIn, const b3Vec3& pNormal);
// Reduce a set of contact points to a quad (approximate convex polygon).
// All points must lie in a common plane and an initial point must be given.
void b3ReducePolygon(b3ClusterPolygon& pOut,
const b3ClusterPolygon& pIn, const b3Vec3& pNormal, u32 initialPoint);
#define B3_NULL_CLUSTER (0xFFFFFFFF)
// An observation represents a contact normal.
struct b3Observation struct b3Observation
{ {
b3Vec3 point;
u32 cluster;
u32 manifold; u32 manifold;
u32 manifoldPoint; u32 manifoldPoint;
b3Vec3 point; // normal
u32 cluster; // normal
}; };
// A group of contact points with a similar contact normal. // A group of contact normals pointing to a similar direction.
struct b3Cluster struct b3Cluster
{ {
b3Vec3 centroid; b3Vec3 centroid;
}; };
// Initialize a set of clusters. class b3ClusterSolver
void b3InitializeClusters(b3Array<b3Cluster>& outClusters, {
const b3Array<b3Observation>& inObservations); public:
b3ClusterSolver();
~b3ClusterSolver();
// Run the cluster algorithm. //
void b3Clusterize(b3Array<b3Cluster>& outClusters, b3Array<b3Observation>& outObservations, void AddObservation(const b3Observation& observation);
const b3Array<b3Cluster>& inClusters, const b3Array<b3Observation>& inObservations);
u32 b3Clusterize(b3Manifold outManifolds[3], const b3Manifold* inManifolds, u32 numIn, //
const b3Transform& xfA, float32 radiusA, const b3Transform& xfB, float32 radiusB); const b3Array<b3Observation>& GetObservations() const;
// Reduce a set of contact points to a quad (approximate convex polygon). //
// All points must lie in a common plane and an initial point must be given. const b3Array<b3Cluster>& GetClusters() const;
void b3ReducePolygon(b3ClusterPolygon& pOut, const b3ClusterPolygon& pIn,
u32 startIndex, const b3Vec3& normal);
#endif //
void Run(b3Manifold mOut[3], u32& numOut,
const b3Manifold* mIn, u32 numIn,
const b3Transform& xfA, float32 radiusA, const b3Transform& xfB, float32 radiusB);
//
void Solve();
private:
//
void InitializeClusters();
//
void AddCluster(const b3Vec3& centroid);
//
u32 BestCluster(const b3Vec3& point) const;
b3StackArray<b3Observation, 32> m_observations;
b3StackArray<b3Cluster, 32> m_clusters;
};
inline void b3ClusterSolver::AddObservation(const b3Observation& observation)
{
m_observations.PushBack(observation);
}
inline const b3Array<b3Observation>& b3ClusterSolver::GetObservations() const
{
return m_observations;
}
inline const b3Array<b3Cluster>& b3ClusterSolver::GetClusters() const
{
return m_clusters;
}
#endif

10
src/bounce/dynamics/contacts/collide/collide_hulls.cpp
View File

@ -132,7 +132,7 @@ void b3BuildFaceContact(b3Manifold& manifold,
// 4. Project the clipped polygon on the reference plane for reduction. // 4. Project the clipped polygon on the reference plane for reduction.
// Ensure the deepest point is contained in the reduced polygon. // Ensure the deepest point is contained in the reduced polygon.
b3StackArray<b3ClusterVertex, 32> polygon1; b3StackArray<b3ClusterPolygonVertex, 32> polygon1;
u32 minIndex = 0; u32 minIndex = 0;
float32 minSeparation = B3_MAX_FLOAT; float32 minSeparation = B3_MAX_FLOAT;
@ -150,7 +150,7 @@ void b3BuildFaceContact(b3Manifold& manifold,
minSeparation = separation; minSeparation = separation;
} }
b3ClusterVertex v1; b3ClusterPolygonVertex v1;
v1.position = b3ClosestPointOnPlane(v2.position, plane1); v1.position = b3ClosestPointOnPlane(v2.position, plane1);
v1.clipIndex = i; v1.clipIndex = i;
polygon1.PushBack(v1); polygon1.PushBack(v1);
@ -167,9 +167,9 @@ void b3BuildFaceContact(b3Manifold& manifold,
// Ensure normal orientation to hull 2. // Ensure normal orientation to hull 2.
b3Vec3 s_normal = flipNormal ? -normal : normal; b3Vec3 s_normal = flipNormal ? -normal : normal;
b3StackArray<b3ClusterVertex, 32> reducedPolygon1; b3StackArray<b3ClusterPolygonVertex, 32> reducedPolygon1;
b3ReducePolygon(reducedPolygon1, polygon1, minIndex, s_normal); b3ReducePolygon(reducedPolygon1, polygon1, s_normal, minIndex);
B3_ASSERT(!reducedPolygon1.IsEmpty()); B3_ASSERT(!reducedPolygon1.IsEmpty());
// 6. Build face contact. // 6. Build face contact.

635
src/bounce/dynamics/contacts/contact_cluster.cpp
View File

@ -19,263 +19,6 @@
#include <bounce/dynamics/contacts/contact_cluster.h> #include <bounce/dynamics/contacts/contact_cluster.h>
#include <bounce/collision/collision.h> #include <bounce/collision/collision.h>
static void AddCluster(b3Array<b3Cluster>& clusters, const b3Vec3& centroid)
{
const float32 kTol = 0.05f;
for (u32 i = 0; i < clusters.Count(); ++i)
{
b3Vec3 c = clusters[i].centroid;
float32 dd = b3DistanceSquared(centroid, c);
if (dd < kTol * kTol)
{
// Merge the clusters.
clusters[i].centroid += centroid;
clusters[i].centroid.Normalize();
return;
}
}
b3Cluster c;
c.centroid = centroid;
clusters.PushBack(c);
}
void b3InitializeClusters(b3Array<b3Cluster>& outClusters, const b3Array<b3Observation>& inObs)
{
B3_ASSERT(outClusters.IsEmpty());
const u32 kMaxClusters = 3;
if (inObs.Count() <= kMaxClusters)
{
for (u32 i = 0; i < inObs.Count(); ++i)
{
const b3Observation& o = inObs[i];
AddCluster(outClusters, o.point);
}
return;
}
B3_ASSERT(inObs.Count() > 3);
// This is used to skip observations that were
// used to initialize a cluster centroid.
b3StackArray<bool, 64> chosens;
chosens.Resize(inObs.Count());
for (u32 i = 0; i < inObs.Count(); ++i)
{
chosens[i] = false;
}
// Choose the most opposing faces.
{
u32 index = 0;
const b3Observation& o = inObs[index];
b3Vec3 A = o.point;
AddCluster(outClusters, A);
chosens[index] = true;
}
{
b3Vec3 A = outClusters[0].centroid;
u32 index = 0;
float32 max = -B3_MAX_FLOAT;
for (u32 i = 0; i < inObs.Count(); ++i)
{
if (chosens[i]) { continue; }
const b3Observation& o = inObs[i];
b3Vec3 B = o.point;
float32 dd = b3DistanceSquared(A, B);
if (dd > max)
{
max = dd;
index = i;
}
}
const b3Observation& o = inObs[index];
b3Vec3 B = o.point;
AddCluster(outClusters, B);
chosens[index] = true;
}
B3_ASSERT(outClusters.Count() == 1 || outClusters.Count() == 2);
if (outClusters.Count() == 1)
{
b3Vec3 A = outClusters[0].centroid;
u32 index = 0;
float32 max = -B3_MAX_FLOAT;
for (u32 i = 0; i < inObs.Count(); ++i)
{
if (chosens[i]) { continue; }
const b3Observation& o = inObs[i];
b3Vec3 B = o.point;
float32 dd = b3DistanceSquared(A, B);
if (dd > max)
{
max = dd;
index = i;
}
}
const b3Observation& o = inObs[index];
b3Vec3 B = o.point;
AddCluster(outClusters, B);
chosens[index] = true;
return;
}
B3_ASSERT(outClusters.Count() == 2);
{
b3Vec3 A = outClusters[0].centroid;
b3Vec3 B = outClusters[1].centroid;
u32 index = 0;
float32 max = -B3_MAX_FLOAT;
for (u32 i = 0; i < inObs.Count(); ++i)
{
if (chosens[i]) { continue; }
const b3Observation& o = inObs[i];
b3Vec3 C = o.point;
b3Vec3 Q = b3ClosestPointOnSegment(C, A, B);
float32 dd = b3DistanceSquared(C, Q);
if (dd > max)
{
max = dd;
index = i;
}
}
const b3Observation& o = inObs[index];
b3Vec3 C = o.point;
AddCluster(outClusters, C);
chosens[index] = true;
}
}
static void b3MoveObsToCluster(b3Array<b3Observation>& observations, u32 fromCluster, u32 toCluster)
{
for (u32 i = 0; i < observations.Count(); ++i)
{
b3Observation& obs = observations[i];
if (obs.cluster == fromCluster)
{
obs.cluster = toCluster;
}
}
}
static u32 b3BestCluster(const b3Array<b3Cluster>& clusters, const b3Vec3& point)
{
u32 bestIndex = 0;
float32 bestValue = B3_MAX_FLOAT;
for (u32 i = 0; i < clusters.Count(); ++i)
{
b3Vec3 centroid = clusters[i].centroid;
float32 metric = b3DistanceSquared(point, centroid);
if (metric < bestValue)
{
bestValue = metric;
bestIndex = i;
}
}
return bestIndex;
}
void b3Clusterize(b3Array<b3Cluster>& outClusters, b3Array<b3Observation>& outObservations,
const b3Array<b3Cluster>& inClusters, const b3Array<b3Observation>& inObservations)
{
B3_ASSERT(outObservations.IsEmpty());
B3_ASSERT(outClusters.IsEmpty());
// Temporary data
b3StackArray<b3Cluster, 32> clusters;
clusters.Swap(inClusters);
b3StackArray<b3Observation, 32> observations;
observations.Swap(inObservations);
// Termination criteria for k-means clustering
const u32 kMaxIters = 10;
u32 iter = 0;
while (iter < kMaxIters)
{
// Assign each observation to the closest cluster centroid.
for (u32 i = 0; i < observations.Count(); ++i)
{
b3Observation& obs = observations[i];
obs.cluster = b3BestCluster(clusters, obs.point);
}
// Compute the new cluster centroids.
for (u32 i = 0; i < clusters.Count(); ++i)
{
b3Cluster& cluster = clusters[i];
b3Vec3 centroid;
centroid.SetZero();
u32 pointCount = 0;
for (u32 j = 0; j < observations.Count(); ++j)
{
const b3Observation& obs = observations[j];
if (obs.cluster == i)
{
centroid += obs.point;
++pointCount;
}
}
if (pointCount > 0)
{
centroid /= float32(pointCount);
cluster.centroid = centroid;
}
}
++iter;
}
// Remove empty clusters.
outObservations.Swap(observations);
u32 numOut = 0;
for (u32 i = 0; i < clusters.Count(); ++i)
{
u32 pointCount = 0;
for (u32 j = 0; j < outObservations.Count(); ++j)
{
const b3Observation& obs = outObservations[j];
if (obs.cluster == i)
{
++pointCount;
}
}
if (pointCount > 0)
{
// Transfer the clusters observations into the newly cluster.
b3MoveObsToCluster(outObservations, i, numOut);
outClusters.PushBack(clusters[i]);
++numOut;
}
}
}
static B3_FORCE_INLINE bool b3IsCCW(const b3Vec3& A, const b3Vec3& B, const b3Vec3& C, const b3Vec3& N) static B3_FORCE_INLINE bool b3IsCCW(const b3Vec3& A, const b3Vec3& B, const b3Vec3& C, const b3Vec3& N)
{ {
b3Vec3 n = b3Cross(B - A, C - A); b3Vec3 n = b3Cross(B - A, C - A);
@ -287,19 +30,24 @@ static B3_FORCE_INLINE bool b3IsCCW(const b3Vec3& A, const b3Vec3& B, const b3Ve
return b3IsCCW(A, B, C, N) && b3IsCCW(C, D, A, N); return b3IsCCW(A, B, C, N) && b3IsCCW(C, D, A, N);
} }
static void b3Weld(b3ClusterPolygon& pOut, const b3Vec3& N) void b3WeldPolygon(b3ClusterPolygon& pOut,
const b3ClusterPolygon& pIn, const b3Vec3& pNormal)
{ {
B3_ASSERT(pOut.Count() > 0); B3_ASSERT(pOut.IsEmpty());
B3_ASSERT(pIn.Count() > 0);
pOut = pIn;
b3Vec3 A = pOut[0].position; b3Vec3 A = pOut[0].position;
for (u32 i = 1; i < pOut.Count(); ++i) for (u32 i = 1; i < pOut.Count(); ++i)
{ {
b3ClusterVertex& vi = pOut[i]; b3ClusterPolygonVertex& vi = pOut[i];
b3Vec3 B = vi.position; b3Vec3 B = vi.position;
for (u32 j = i + 1; j < pOut.Count(); ++j) for (u32 j = i + 1; j < pOut.Count(); ++j)
{ {
b3ClusterVertex& vj = pOut[j]; b3ClusterPolygonVertex& vj = pOut[j];
b3Vec3 C = vj.position; b3Vec3 C = vj.position;
if (b3IsCCW(A, B, C, N) == false) if (b3IsCCW(A, B, C, pNormal) == false)
{ {
b3Swap(vi, vj); b3Swap(vi, vj);
} }
@ -307,23 +55,22 @@ static void b3Weld(b3ClusterPolygon& pOut, const b3Vec3& N)
} }
} }
void b3ReducePolygon(b3ClusterPolygon& pOut, void b3ReducePolygon(b3ClusterPolygon& pOut,
const b3ClusterPolygon& pIn, u32 startIndex, const b3Vec3& normal) const b3ClusterPolygon& pIn, const b3Vec3& pNormal, u32 initialPoint)
{ {
B3_ASSERT(pIn.Count() > 0); B3_ASSERT(pIn.Count() > 0);
B3_ASSERT(pOut.Count() == 0); B3_ASSERT(pOut.Count() == 0);
B3_ASSERT(startIndex < pIn.Count()); B3_ASSERT(initialPoint < pIn.Count());
pOut.Reserve(pIn.Count()); pOut.Reserve(pIn.Count());
if (pIn.Count() <= B3_MAX_MANIFOLD_POINTS) if (pIn.Count() <= B3_MAX_MANIFOLD_POINTS)
{ {
pOut = pIn; b3WeldPolygon(pOut, pIn, pNormal);
b3Weld(pOut, normal);
return; return;
} }
B3_ASSERT(pIn.Count() > B3_MAX_MANIFOLD_POINTS); B3_ASSERT(pIn.Count() > B3_MAX_MANIFOLD_POINTS);
b3StackArray<bool, 32> chosens; b3StackArray<bool, 32> chosens;
chosens.Resize(pIn.Count()); chosens.Resize(pIn.Count());
for (u32 i = 0; i < chosens.Count(); ++i) for (u32 i = 0; i < chosens.Count(); ++i)
@ -332,7 +79,7 @@ void b3ReducePolygon(b3ClusterPolygon& pOut,
} }
{ {
u32 index = startIndex; u32 index = initialPoint;
pOut.PushBack(pIn[index]); pOut.PushBack(pIn[index]);
chosens[index] = true; chosens[index] = true;
} }
@ -378,7 +125,7 @@ void b3ReducePolygon(b3ClusterPolygon& pOut,
b3Vec3 C = pIn[i].position; b3Vec3 C = pIn[i].position;
b3Vec3 N = b3Cross(B - A, C - A); b3Vec3 N = b3Cross(B - A, C - A);
float32 sa2 = b3Dot(N, normal); float32 sa2 = b3Dot(N, pNormal);
float32 a2 = b3Abs(sa2); float32 a2 = b3Abs(sa2);
if (a2 > max) if (a2 > max)
{ {
@ -398,7 +145,7 @@ void b3ReducePolygon(b3ClusterPolygon& pOut,
// Ensure CCW order of triangle ABC. // Ensure CCW order of triangle ABC.
b3Vec3 C = pIn[index].position; b3Vec3 C = pIn[index].position;
if (b3IsCCW(A, B, C, normal) == false) if (b3IsCCW(A, B, C, pNormal) == false)
{ {
b3Swap(pOut[0], pOut[1]); b3Swap(pOut[0], pOut[1]);
} }
@ -412,7 +159,7 @@ void b3ReducePolygon(b3ClusterPolygon& pOut,
b3Vec3 B = pOut[1].position; b3Vec3 B = pOut[1].position;
b3Vec3 C = pOut[2].position; b3Vec3 C = pOut[2].position;
B3_ASSERT(b3IsCCW(A, B, C, normal)); B3_ASSERT(b3IsCCW(A, B, C, pNormal));
u32 index = 0; u32 index = 0;
float32 min = B3_MAX_FLOAT; float32 min = B3_MAX_FLOAT;
@ -422,7 +169,7 @@ void b3ReducePolygon(b3ClusterPolygon& pOut,
b3Vec3 D = pIn[i].position; b3Vec3 D = pIn[i].position;
b3Vec3 N = b3Cross(B - A, D - A); b3Vec3 N = b3Cross(B - A, D - A);
float32 sa2 = b3Dot(N, normal); float32 sa2 = b3Dot(N, pNormal);
if (sa2 < min) if (sa2 < min)
{ {
min = sa2; min = sa2;
@ -442,17 +189,292 @@ void b3ReducePolygon(b3ClusterPolygon& pOut,
chosens[index] = true; chosens[index] = true;
} }
// Weld output polygon
B3_ASSERT(pOut.Count() <= B3_MAX_MANIFOLD_POINTS); B3_ASSERT(pOut.Count() <= B3_MAX_MANIFOLD_POINTS);
b3Weld(pOut, normal);
b3StackArray<b3ClusterPolygonVertex, 32> quad;
b3WeldPolygon(quad, pOut, pNormal);
// Output polygon
pOut = quad;
} }
u32 b3Clusterize(b3Manifold outManifolds[3], const b3Manifold* inManifolds, u32 numIn, b3ClusterSolver::b3ClusterSolver()
{
}
b3ClusterSolver::~b3ClusterSolver()
{
}
void b3ClusterSolver::InitializeClusters()
{
B3_ASSERT(m_clusters.IsEmpty());
const u32 kMaxClusters = 3;
if (m_observations.Count() <= kMaxClusters)
{
for (u32 i = 0; i < m_observations.Count(); ++i)
{
AddCluster(m_observations[i].point);
}
return;
}
B3_ASSERT(m_observations.Count() > 3);
// This is used to skip observations that were
// used to initialize a cluster centroid.
b3StackArray<bool, 64> chosens;
chosens.Resize(m_observations.Count());
for (u32 i = 0; i < m_observations.Count(); ++i)
{
chosens[i] = false;
}
// Choose the most opposing faces.
{
u32 index = 0;
const b3Observation& o = m_observations[index];
b3Vec3 A = o.point;
AddCluster(A);
chosens[index] = true;
}
{
b3Vec3 A = m_clusters[0].centroid;
u32 index = 0;
float32 max = -B3_MAX_FLOAT;
for (u32 i = 0; i < m_observations.Count(); ++i)
{
if (chosens[i]) { continue; }
const b3Observation& o = m_observations[i];
b3Vec3 B = o.point;
float32 dd = b3DistanceSquared(A, B);
if (dd > max)
{
max = dd;
index = i;
}
}
const b3Observation& o = m_observations[index];
b3Vec3 B = o.point;
AddCluster(B);
chosens[index] = true;
}
B3_ASSERT(m_clusters.Count() == 1 || m_clusters.Count() == 2);
if (m_clusters.Count() == 1)
{
b3Vec3 A = m_clusters[0].centroid;
u32 index = 0;
float32 max = -B3_MAX_FLOAT;
for (u32 i = 0; i < m_observations.Count(); ++i)
{
if (chosens[i]) { continue; }
const b3Observation& o = m_observations[i];
b3Vec3 B = o.point;
float32 dd = b3DistanceSquared(A, B);
if (dd > max)
{
max = dd;
index = i;
}
}
const b3Observation& o = m_observations[index];
b3Vec3 B = o.point;
AddCluster(B);
chosens[index] = true;
return;
}
B3_ASSERT(m_clusters.Count() == 2);
{
b3Vec3 A = m_clusters[0].centroid;
b3Vec3 B = m_clusters[1].centroid;
u32 index = 0;
float32 max = -B3_MAX_FLOAT;
for (u32 i = 0; i < m_observations.Count(); ++i)
{
if (chosens[i]) { continue; }
const b3Observation& o = m_observations[i];
b3Vec3 C = o.point;
b3Vec3 Q = b3ClosestPointOnSegment(C, A, B);
float32 dd = b3DistanceSquared(C, Q);
if (dd > max)
{
max = dd;
index = i;
}
}
const b3Observation& o = m_observations[index];
b3Vec3 C = o.point;
AddCluster(C);
chosens[index] = true;
}
}
void b3ClusterSolver::AddCluster(const b3Vec3& centroid)
{
if (m_clusters.IsEmpty())
{
// Add a new cluster
b3Cluster c;
c.centroid = centroid;
m_clusters.PushBack(c);
return;
}
u32 bestIndex = BestCluster(centroid);
b3Cluster& bestCluster = m_clusters[bestIndex];
// Should we merge the cluster?
const float32 kTol = 0.05f;
if (b3DistanceSquared(centroid, bestCluster.centroid) <= kTol * kTol)
{
// Merge the clusters
bestCluster.centroid += centroid;
bestCluster.centroid.Normalize();
return;
}
// Add a new cluster
b3Cluster c;
c.centroid = centroid;
m_clusters.PushBack(c);
}
u32 b3ClusterSolver::BestCluster(const b3Vec3& point) const
{
u32 bestIndex = 0;
float32 bestValue = B3_MAX_FLOAT;
for (u32 i = 0; i < m_clusters.Count(); ++i)
{
b3Vec3 centroid = m_clusters[i].centroid;
float32 metric = b3DistanceSquared(point, centroid);
if (metric < bestValue)
{
bestValue = metric;
bestIndex = i;
}
}
return bestIndex;
}
static void b3MoveObservationsToCluster(b3Array<b3Observation>& observations, u32 fromCluster, u32 toCluster)
{
for (u32 i = 0; i < observations.Count(); ++i)
{
b3Observation& obs = observations[i];
if (obs.cluster == fromCluster)
{
obs.cluster = toCluster;
}
}
}
void b3ClusterSolver::Solve()
{
// Initialize clusters
InitializeClusters();
// Run k-means
// Termination criteria
const u32 kMaxIters = 10;
u32 iter = 0;
while (iter < kMaxIters)
{
// Assign each observation to the closest cluster centroid.
for (u32 i = 0; i < m_observations.Count(); ++i)
{
b3Observation& obs = m_observations[i];
obs.cluster = BestCluster(obs.point);
}
// Compute the new cluster centroids.
for (u32 i = 0; i < m_clusters.Count(); ++i)
{
b3Cluster& cluster = m_clusters[i];
// Center
b3Vec3 centroid;
centroid.SetZero();
u32 pointCount = 0;
for (u32 j = 0; j < m_observations.Count(); ++j)
{
const b3Observation& obs = m_observations[j];
if (obs.cluster == i)
{
centroid += obs.point;
++pointCount;
}
}
if (pointCount > 0)
{
centroid /= float32(pointCount);
cluster.centroid = centroid;
}
}
++iter;
}
// Remove empty clusters
b3StackArray<b3Cluster, 32> usedClusters;
for (u32 i = 0; i < m_clusters.Count(); ++i)
{
// Count the observations in the ith cluster
u32 obsCount = 0;
for (u32 j = 0; j < m_observations.Count(); ++j)
{
const b3Observation& obs = m_observations[j];
if (obs.cluster == i)
{
++obsCount;
}
}
if (obsCount > 0)
{
// Copy the clusters observations into the new cluster.
b3MoveObservationsToCluster(m_observations, i, usedClusters.Count());
usedClusters.PushBack(m_clusters[i]);
}
}
m_clusters = usedClusters;
}
void b3ClusterSolver::Run(b3Manifold outManifolds[3], u32& numOut,
const b3Manifold* inManifolds, u32 numIn,
const b3Transform& xfA, float32 radiusA, const b3Transform& xfB, float32 radiusB) const b3Transform& xfA, float32 radiusA, const b3Transform& xfB, float32 radiusB)
{ {
u32 numOut = 0; // Initialize observations
// Initialize normals
b3StackArray<b3Observation, 32> tempObservations;
for (u32 i = 0; i < numIn; ++i) for (u32 i = 0; i < numIn; ++i)
{ {
b3WorldManifold wm; b3WorldManifold wm;
@ -460,28 +482,24 @@ u32 b3Clusterize(b3Manifold outManifolds[3], const b3Manifold* inManifolds, u32
for (u32 j = 0; j < wm.pointCount; ++j) for (u32 j = 0; j < wm.pointCount; ++j)
{ {
b3Observation obs; b3Observation o;
obs.manifold = i; o.point = wm.points[j].normal;
obs.manifoldPoint = j; o.cluster = B3_NULL_CLUSTER;
obs.point = wm.points[j].normal; o.manifold = i;
obs.cluster = B3_NULL_CLUSTER; o.manifoldPoint = j;
tempObservations.PushBack(obs); m_observations.PushBack(o);
} }
} }
// Initialize clusters // Solve
b3StackArray<b3Cluster, 3> tempClusters; Solve();
b3InitializeClusters(tempClusters, tempObservations);
// Cluster // Reduce, weld, and output contact manifold
b3StackArray<b3Cluster, 3> clusters;
b3StackArray<b3Observation, 32> observations;
b3Clusterize(clusters, observations, tempClusters, tempObservations);
B3_ASSERT(clusters.Count() <= 3); B3_ASSERT(m_clusters.Count() <= B3_MAX_MANIFOLDS);
for (u32 i = 0; i < clusters.Count(); ++i) for (u32 i = 0; i < m_clusters.Count(); ++i)
{ {
// Gather manifold points. // Gather manifold points.
b3Vec3 center; b3Vec3 center;
@ -490,10 +508,10 @@ u32 b3Clusterize(b3Manifold outManifolds[3], const b3Manifold* inManifolds, u32
b3Vec3 normal; b3Vec3 normal;
normal.SetZero(); normal.SetZero();
b3StackArray<b3ClusterVertex, 32> polygonB; b3StackArray<b3ClusterPolygonVertex, 32> polygonB;
for (u32 j = 0; j < observations.Count(); ++j) for (u32 j = 0; j < m_observations.Count(); ++j)
{ {
b3Observation& o = observations[j]; b3Observation& o = m_observations[j];
if (o.cluster != i) if (o.cluster != i)
{ {
continue; continue;
@ -505,7 +523,7 @@ u32 b3Clusterize(b3Manifold outManifolds[3], const b3Manifold* inManifolds, u32
b3WorldManifoldPoint wmp; b3WorldManifoldPoint wmp;
wmp.Initialize(mp, radiusA, xfA, radiusB, xfB); wmp.Initialize(mp, radiusA, xfA, radiusB, xfB);
b3ClusterVertex cv; b3ClusterPolygonVertex cv;
cv.position = wmp.point; cv.position = wmp.point;
cv.clipIndex = j; cv.clipIndex = j;
polygonB.PushBack(cv); polygonB.PushBack(cv);
@ -532,7 +550,7 @@ u32 b3Clusterize(b3Manifold outManifolds[3], const b3Manifold* inManifolds, u32
float32 minSeparation = B3_MAX_FLOAT; float32 minSeparation = B3_MAX_FLOAT;
for (u32 j = 0; j < polygonB.Count(); ++j) for (u32 j = 0; j < polygonB.Count(); ++j)
{ {
const b3Observation* o = observations.Get(polygonB[j].clipIndex); const b3Observation* o = m_observations.Get(polygonB[j].clipIndex);
const b3Manifold* inManifold = inManifolds + o->manifold; const b3Manifold* inManifold = inManifolds + o->manifold;
const b3ManifoldPoint* inPoint = inManifold->points + o->manifoldPoint; const b3ManifoldPoint* inPoint = inManifold->points + o->manifoldPoint;
@ -549,23 +567,22 @@ u32 b3Clusterize(b3Manifold outManifolds[3], const b3Manifold* inManifolds, u32
polygonB[j].position = polygonB[j].position + separation * normal; polygonB[j].position = polygonB[j].position + separation * normal;
} }
b3StackArray<b3ClusterVertex, 32> reducedB; b3StackArray<b3ClusterPolygonVertex, 32> quadB;
b3ReducePolygon(reducedB, polygonB, minIndex, normal); b3ReducePolygon(quadB, polygonB, normal, minIndex);
for (u32 j = 0; j < reducedB.Count(); ++j) for (u32 j = 0; j < quadB.Count(); ++j)
{ {
b3ClusterVertex v = reducedB[j]; b3ClusterPolygonVertex v = quadB[j];
u32 inIndex = v.clipIndex; u32 inIndex = v.clipIndex;
const b3Observation* o = observations.Get(inIndex); const b3Observation* o = m_observations.Get(inIndex);
const b3Manifold* inManifold = inManifolds + o->manifold; const b3Manifold* inManifold = inManifolds + o->manifold;
const b3ManifoldPoint* inPoint = inManifold->points + o->manifoldPoint; const b3ManifoldPoint* inPoint = inManifold->points + o->manifoldPoint;
manifold->points[j] = *inPoint; manifold->points[j] = *inPoint;
} }
manifold->pointCount = reducedB.Count(); manifold->pointCount = quadB.Count();
} }
B3_ASSERT(numOut <= B3_MAX_MANIFOLDS); B3_ASSERT(numOut <= B3_MAX_MANIFOLDS);
return numOut; }
}

4
src/bounce/dynamics/contacts/mesh_contact.cpp
View File

@ -267,7 +267,9 @@ void b3MeshContact::Collide()
// Send contact manifolds for clustering. This is an important optimization. // Send contact manifolds for clustering. This is an important optimization.
B3_ASSERT(m_manifoldCount == 0); B3_ASSERT(m_manifoldCount == 0);
m_manifoldCount = b3Clusterize(m_stackManifolds, tempManifolds, tempCount, xfA, shapeA->m_radius, xfB, B3_HULL_RADIUS);
b3ClusterSolver clusterSolver;
clusterSolver.Run(m_stackManifolds, m_manifoldCount, tempManifolds, tempCount, xfA, shapeA->m_radius, xfB, B3_HULL_RADIUS);
allocator->Free(tempManifolds); allocator->Free(tempManifolds);
} }