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

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

@ -23,47 +23,92 @@
#include <bounce/common/template/array.h>
#include <bounce/dynamics/contacts/manifold.h>
#define B3_NULL_CLUSTER (0xFFFFFFFF)
// Used for contact cluster reduction.
struct b3ClusterVertex
struct b3ClusterPolygonVertex
{
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.
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
{
b3Vec3 point;
u32 cluster;
u32 manifold;
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
{
b3Vec3 centroid;
};
// Initialize a set of clusters.
void b3InitializeClusters(b3Array<b3Cluster>& outClusters,
const b3Array<b3Observation>& inObservations);
class b3ClusterSolver
{
public:
b3ClusterSolver();
~b3ClusterSolver();
// Run the cluster algorithm.
void b3Clusterize(b3Array<b3Cluster>& outClusters, b3Array<b3Observation>& outObservations,
const b3Array<b3Cluster>& inClusters, const b3Array<b3Observation>& inObservations);
//
void AddObservation(const b3Observation& observation);
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.
void b3ReducePolygon(b3ClusterPolygon& pOut, const b3ClusterPolygon& pIn,
u32 startIndex, const b3Vec3& normal);
//
const b3Array<b3Cluster>& GetClusters() const;
#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.
// Ensure the deepest point is contained in the reduced polygon.
b3StackArray<b3ClusterVertex, 32> polygon1;
b3StackArray<b3ClusterPolygonVertex, 32> polygon1;
u32 minIndex = 0;
float32 minSeparation = B3_MAX_FLOAT;
@ -150,7 +150,7 @@ void b3BuildFaceContact(b3Manifold& manifold,
minSeparation = separation;
}
b3ClusterVertex v1;
b3ClusterPolygonVertex v1;
v1.position = b3ClosestPointOnPlane(v2.position, plane1);
v1.clipIndex = i;
polygon1.PushBack(v1);
@ -167,9 +167,9 @@ void b3BuildFaceContact(b3Manifold& manifold,
// Ensure normal orientation to hull 2.
b3Vec3 s_normal = flipNormal ? -normal : normal;
b3StackArray<b3ClusterVertex, 32> reducedPolygon1;
b3ReducePolygon(reducedPolygon1, polygon1, minIndex, s_normal);
b3StackArray<b3ClusterPolygonVertex, 32> reducedPolygon1;
b3ReducePolygon(reducedPolygon1, polygon1, s_normal, minIndex);
B3_ASSERT(!reducedPolygon1.IsEmpty());
// 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/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)
{
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);
}
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;
for (u32 i = 1; i < pOut.Count(); ++i)
{
b3ClusterVertex& vi = pOut[i];
b3ClusterPolygonVertex& vi = pOut[i];
b3Vec3 B = vi.position;
for (u32 j = i + 1; j < pOut.Count(); ++j)
{
b3ClusterVertex& vj = pOut[j];
b3ClusterPolygonVertex& vj = pOut[j];
b3Vec3 C = vj.position;
if (b3IsCCW(A, B, C, N) == false)
if (b3IsCCW(A, B, C, pNormal) == false)
{
b3Swap(vi, vj);
}
@ -307,23 +55,22 @@ static void b3Weld(b3ClusterPolygon& pOut, const b3Vec3& N)
}
}
void b3ReducePolygon(b3ClusterPolygon& pOut,
const b3ClusterPolygon& pIn, u32 startIndex, const b3Vec3& normal)
void b3ReducePolygon(b3ClusterPolygon& pOut,
const b3ClusterPolygon& pIn, const b3Vec3& pNormal, u32 initialPoint)
{
B3_ASSERT(pIn.Count() > 0);
B3_ASSERT(pOut.Count() == 0);
B3_ASSERT(startIndex < pIn.Count());
B3_ASSERT(initialPoint < pIn.Count());
pOut.Reserve(pIn.Count());
if (pIn.Count() <= B3_MAX_MANIFOLD_POINTS)
{
pOut = pIn;
b3Weld(pOut, normal);
b3WeldPolygon(pOut, pIn, pNormal);
return;
}
B3_ASSERT(pIn.Count() > B3_MAX_MANIFOLD_POINTS);
b3StackArray<bool, 32> chosens;
chosens.Resize(pIn.Count());
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]);
chosens[index] = true;
}
@ -378,7 +125,7 @@ void b3ReducePolygon(b3ClusterPolygon& pOut,
b3Vec3 C = pIn[i].position;
b3Vec3 N = b3Cross(B - A, C - A);
float32 sa2 = b3Dot(N, normal);
float32 sa2 = b3Dot(N, pNormal);
float32 a2 = b3Abs(sa2);
if (a2 > max)
{
@ -398,7 +145,7 @@ void b3ReducePolygon(b3ClusterPolygon& pOut,
// Ensure CCW order of triangle ABC.
b3Vec3 C = pIn[index].position;
if (b3IsCCW(A, B, C, normal) == false)
if (b3IsCCW(A, B, C, pNormal) == false)
{
b3Swap(pOut[0], pOut[1]);
}
@ -412,7 +159,7 @@ void b3ReducePolygon(b3ClusterPolygon& pOut,
b3Vec3 B = pOut[1].position;
b3Vec3 C = pOut[2].position;
B3_ASSERT(b3IsCCW(A, B, C, normal));
B3_ASSERT(b3IsCCW(A, B, C, pNormal));
u32 index = 0;
float32 min = B3_MAX_FLOAT;
@ -422,7 +169,7 @@ void b3ReducePolygon(b3ClusterPolygon& pOut,
b3Vec3 D = pIn[i].position;
b3Vec3 N = b3Cross(B - A, D - A);
float32 sa2 = b3Dot(N, normal);
float32 sa2 = b3Dot(N, pNormal);
if (sa2 < min)
{
min = sa2;
@ -442,17 +189,292 @@ void b3ReducePolygon(b3ClusterPolygon& pOut,
chosens[index] = true;
}
// Weld output polygon
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)
{
u32 numOut = 0;
// Initialize normals
b3StackArray<b3Observation, 32> tempObservations;
// Initialize observations
for (u32 i = 0; i < numIn; ++i)
{
b3WorldManifold wm;
@ -460,28 +482,24 @@ u32 b3Clusterize(b3Manifold outManifolds[3], const b3Manifold* inManifolds, u32
for (u32 j = 0; j < wm.pointCount; ++j)
{
b3Observation obs;
obs.manifold = i;
obs.manifoldPoint = j;
obs.point = wm.points[j].normal;
obs.cluster = B3_NULL_CLUSTER;
tempObservations.PushBack(obs);
b3Observation o;
o.point = wm.points[j].normal;
o.cluster = B3_NULL_CLUSTER;
o.manifold = i;
o.manifoldPoint = j;
m_observations.PushBack(o);
}
}
// Initialize clusters
b3StackArray<b3Cluster, 3> tempClusters;
b3InitializeClusters(tempClusters, tempObservations);
// Solve
Solve();
// Cluster
b3StackArray<b3Cluster, 3> clusters;
b3StackArray<b3Observation, 32> observations;
b3Clusterize(clusters, observations, tempClusters, tempObservations);
// Reduce, weld, and output contact manifold
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.
b3Vec3 center;
@ -490,10 +508,10 @@ u32 b3Clusterize(b3Manifold outManifolds[3], const b3Manifold* inManifolds, u32
b3Vec3 normal;
normal.SetZero();
b3StackArray<b3ClusterVertex, 32> polygonB;
for (u32 j = 0; j < observations.Count(); ++j)
b3StackArray<b3ClusterPolygonVertex, 32> polygonB;
for (u32 j = 0; j < m_observations.Count(); ++j)
{
b3Observation& o = observations[j];
b3Observation& o = m_observations[j];
if (o.cluster != i)
{
continue;
@ -505,7 +523,7 @@ u32 b3Clusterize(b3Manifold outManifolds[3], const b3Manifold* inManifolds, u32
b3WorldManifoldPoint wmp;
wmp.Initialize(mp, radiusA, xfA, radiusB, xfB);
b3ClusterVertex cv;
b3ClusterPolygonVertex cv;
cv.position = wmp.point;
cv.clipIndex = j;
polygonB.PushBack(cv);
@ -532,7 +550,7 @@ u32 b3Clusterize(b3Manifold outManifolds[3], const b3Manifold* inManifolds, u32
float32 minSeparation = B3_MAX_FLOAT;
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 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;
}
b3StackArray<b3ClusterVertex, 32> reducedB;
b3ReducePolygon(reducedB, polygonB, minIndex, normal);
for (u32 j = 0; j < reducedB.Count(); ++j)
b3StackArray<b3ClusterPolygonVertex, 32> quadB;
b3ReducePolygon(quadB, polygonB, normal, minIndex);
for (u32 j = 0; j < quadB.Count(); ++j)
{
b3ClusterVertex v = reducedB[j];
b3ClusterPolygonVertex v = quadB[j];
u32 inIndex = v.clipIndex;
const b3Observation* o = observations.Get(inIndex);
const b3Observation* o = m_observations.Get(inIndex);
const b3Manifold* inManifold = inManifolds + o->manifold;
const b3ManifoldPoint* inPoint = inManifold->points + o->manifoldPoint;
manifold->points[j] = *inPoint;
}
manifold->pointCount = reducedB.Count();
manifold->pointCount = quadB.Count();
}
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.
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);
}