From 5a801717445867a3756dda63991e380719109160 Mon Sep 17 00:00:00 2001 From: Irlan <-> Date: Fri, 23 Mar 2018 00:12:59 -0300 Subject: [PATCH] improve contact clustering algorithm --- examples/testbed/tests/cluster_test.h | 44 +- .../dynamics/contacts/contact_cluster.h | 89 ++- .../contacts/collide/collide_hulls.cpp | 10 +- .../dynamics/contacts/contact_cluster.cpp | 635 +++++++++--------- src/bounce/dynamics/contacts/mesh_contact.cpp | 4 +- 5 files changed, 425 insertions(+), 357 deletions(-) diff --git a/examples/testbed/tests/cluster_test.h b/examples/testbed/tests/cluster_test.h index a1896e6..3c05c59 100644 --- a/examples/testbed/tests/cluster_test.h +++ b/examples/testbed/tests/cluster_test.h @@ -32,27 +32,28 @@ public: g_camera.m_zoom = 10.0f; // Initialize observations - b3StackArray 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 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& 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& observations = m_solver.GetObservations(); + const b3Array& 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 m_observs; - b3StackArray m_clusters; - b3Color m_colors[3]; + b3ClusterSolver m_solver; + + b3StackArray m_colors; }; #endif diff --git a/include/bounce/dynamics/contacts/contact_cluster.h b/include/bounce/dynamics/contacts/contact_cluster.h index d4649bd..e40b0b7 100644 --- a/include/bounce/dynamics/contacts/contact_cluster.h +++ b/include/bounce/dynamics/contacts/contact_cluster.h @@ -23,47 +23,92 @@ #include #include -#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 b3ClusterPolygon; +typedef b3Array 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& outClusters, -const b3Array& inObservations); +class b3ClusterSolver +{ +public: + b3ClusterSolver(); + + ~b3ClusterSolver(); -// Run the cluster algorithm. -void b3Clusterize(b3Array& outClusters, b3Array& outObservations, - const b3Array& inClusters, const b3Array& 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& 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& 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 m_observations; + b3StackArray m_clusters; +}; + +inline void b3ClusterSolver::AddObservation(const b3Observation& observation) +{ + m_observations.PushBack(observation); +} + +inline const b3Array& b3ClusterSolver::GetObservations() const +{ + return m_observations; +} + +inline const b3Array& b3ClusterSolver::GetClusters() const +{ + return m_clusters; +} + +#endif \ No newline at end of file diff --git a/src/bounce/dynamics/contacts/collide/collide_hulls.cpp b/src/bounce/dynamics/contacts/collide/collide_hulls.cpp index 6b59880..67759bf 100644 --- a/src/bounce/dynamics/contacts/collide/collide_hulls.cpp +++ b/src/bounce/dynamics/contacts/collide/collide_hulls.cpp @@ -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 polygon1; + b3StackArray 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 reducedPolygon1; - b3ReducePolygon(reducedPolygon1, polygon1, minIndex, s_normal); + + b3StackArray reducedPolygon1; + b3ReducePolygon(reducedPolygon1, polygon1, s_normal, minIndex); B3_ASSERT(!reducedPolygon1.IsEmpty()); // 6. Build face contact. diff --git a/src/bounce/dynamics/contacts/contact_cluster.cpp b/src/bounce/dynamics/contacts/contact_cluster.cpp index bd9474a..e1e7862 100644 --- a/src/bounce/dynamics/contacts/contact_cluster.cpp +++ b/src/bounce/dynamics/contacts/contact_cluster.cpp @@ -19,263 +19,6 @@ #include #include -static void AddCluster(b3Array& 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& outClusters, const b3Array& 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 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& 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& 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& outClusters, b3Array& outObservations, - const b3Array& inClusters, const b3Array& inObservations) -{ - B3_ASSERT(outObservations.IsEmpty()); - B3_ASSERT(outClusters.IsEmpty()); - - // Temporary data - b3StackArray clusters; - clusters.Swap(inClusters); - - b3StackArray 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 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 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 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& 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 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 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 tempClusters; - b3InitializeClusters(tempClusters, tempObservations); + // Solve + Solve(); - // Cluster - b3StackArray clusters; - b3StackArray 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 polygonB; - for (u32 j = 0; j < observations.Count(); ++j) + b3StackArray 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 reducedB; - b3ReducePolygon(reducedB, polygonB, minIndex, normal); - for (u32 j = 0; j < reducedB.Count(); ++j) + b3StackArray 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; -} +} \ No newline at end of file diff --git a/src/bounce/dynamics/contacts/mesh_contact.cpp b/src/bounce/dynamics/contacts/mesh_contact.cpp index 1f7169a..892f31f 100644 --- a/src/bounce/dynamics/contacts/mesh_contact.cpp +++ b/src/bounce/dynamics/contacts/mesh_contact.cpp @@ -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); } \ No newline at end of file