Added contact point reduction for virtual character to avoid getting stuck on complex meshes (#377)

* Removed max hits limit for ContactCastCollector
* Implemented hit reduction algorithm to fix character getting stuck when exceeding the max hit limit
* Added half cylinder for testing contact point reduction
* Added unit test for character contact point reduction

Jolt/Physics/Character/CharacterVirtual.cpp

@@ -27,6 +27,7 @@ CharacterVirtual::CharacterVirtual(const CharacterVirtualSettings *inSettings, R
 	mCollisionTolerance(inSettings->mCollisionTolerance),
 	mCharacterPadding(inSettings->mCharacterPadding),
 	mMaxNumHits(inSettings->mMaxNumHits),
+	mHitReductionCosMaxAngle(inSettings->mHitReductionCosMaxAngle),
 	mPenetrationRecoverySpeed(inSettings->mPenetrationRecoverySpeed),
 	mShapeOffset(inSettings->mShapeOffset),
 	mPosition(inPosition),
@@ -58,6 +59,60 @@ void CharacterVirtual::sFillContactProperties(Contact &outContact, const Body &i
 
 void CharacterVirtual::ContactCollector::AddHit(const CollideShapeResult &inResult)
 {
+	// If we exceed our contact limit, try to clean up near-duplicate contacts
+	if (mContacts.size() == mMaxHits)
+	{
+		// Flag that we hit this code path
+		mMaxHitsExceeded = true;
+
+		// Check if we can do reduction
+		if (mHitReductionCosMaxAngle > -1.0f)
+		{
+			// Loop all contacts and find similar contacts
+			for (int i = (int)mContacts.size() - 1; i >= 0; --i)
+			{
+				Contact &contact_i = mContacts[i];
+				for (int j = i - 1; j >= 0; --j)
+				{
+					Contact &contact_j = mContacts[j];
+					if (contact_i.mBodyB == contact_j.mBodyB // Same body
+						&& contact_i.mContactNormal.Dot(contact_j.mContactNormal) > mHitReductionCosMaxAngle) // Very similar contact normals
+					{
+						// Remove the contact with the biggest distance
+						bool i_is_last = i == (int)mContacts.size() - 1;
+						if (contact_i.mDistance > contact_j.mDistance)
+						{
+							// Remove i
+							if (!i_is_last)
+								contact_i = mContacts.back();
+							mContacts.pop_back();
+
+							// Break out of the loop, i is now an element that we already processed
+							break;
+						}
+						else
+						{
+							// Remove j
+							contact_j = mContacts.back();
+							mContacts.pop_back();
+
+							// If i was the last element, we just moved it into position j. Break out of the loop, we'll see it again later.
+							if (i_is_last)
+								break;
+						}
+					}
+				}
+			}
+		}
+
+		if (mContacts.size() == mMaxHits)
+		{
+			// There are still too many hits, give up!
+			ForceEarlyOut();
+			return;
+		}
+	}
+
 	BodyLockRead lock(mSystem->GetBodyLockInterface(), inResult.mBodyID2);
 	if (lock.SucceededAndIsInBroadPhase())
 	{
@@ -67,15 +122,14 @@ void CharacterVirtual::ContactCollector::AddHit(const CollideShapeResult &inResu
 		Contact &contact = mContacts.back();
 		sFillContactProperties(contact, body, mUp, mBaseOffset, *this, inResult);
 		contact.mFraction = 0.0f;
-
-		// Protection from excess of contact points
-		if (mContacts.size() == mMaxHits)
-			ForceEarlyOut();
 	}
 }
 
 void CharacterVirtual::ContactCastCollector::AddHit(const ShapeCastResult &inResult)
 {
+	// Should not have gotten here without a lower fraction
+	JPH_ASSERT(inResult.mFraction < mContact.mFraction);
+
 	if (inResult.mFraction > 0.0f // Ignore collisions at fraction = 0
 		&& inResult.mPenetrationAxis.Dot(mDisplacement) > 0.0f) // Ignore penetrations that we're moving away from
 	{
@@ -89,14 +143,18 @@ void CharacterVirtual::ContactCastCollector::AddHit(const ShapeCastResult &inRes
 		{
 			const Body &body = lock.GetBody();
 
-			mContacts.emplace_back();
-			Contact &contact = mContacts.back();
+			// Convert the hit result into a contact
+			Contact contact;
 			sFillContactProperties(contact, body, mUp, mBaseOffset, *this, inResult);
 			contact.mFraction = inResult.mFraction;
-
-			// Protection from excess of contact points
-			if (mContacts.size() == mMaxHits)
-				ForceEarlyOut();
+			
+			// Check if the contact that will make us penetrate more than the allowed tolerance
+			if (contact.mDistance + contact.mContactNormal.Dot(mDisplacement) < -mCharacter->mCollisionTolerance
+				&& mCharacter->ValidateContact(contact))
+			{
+				mContact = contact;
+				UpdateEarlyOutFraction(contact.mFraction);
+			}
 		}
 	}
 }
@@ -123,9 +181,12 @@ void CharacterVirtual::GetContactsAtPosition(RVec3Arg inPosition, Vec3Arg inMove
 	outContacts.clear();
 
 	// Collide shape
-	ContactCollector collector(mSystem, mMaxNumHits, mUp, mPosition, outContacts);
+	ContactCollector collector(mSystem, mMaxNumHits, mHitReductionCosMaxAngle, mUp, mPosition, outContacts);
 	CheckCollision(inPosition, mRotation, inMovementDirection, mPredictiveContactDistance, inShape, mPosition, collector, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter);
 
+	// Flag if we exceeded the max number of hits
+	mMaxHitsExceeded = collector.mMaxHitsExceeded;
+
 	// Reduce distance to contact by padding to ensure we stay away from the object by a little margin
 	// (this will make collision detection cheaper - especially for sweep tests as they won't hit the surface if we're properly sliding)
 	for (Contact &c : outContacts)
@@ -205,7 +266,7 @@ inline static bool sCorrectFractionForCharacterPadding(const Shape *inShape, Mat
 	}
 }
 
-bool CharacterVirtual::GetFirstContactForSweep(RVec3Arg inPosition, Vec3Arg inDisplacement, Contact &outContact, const IgnoredContactList &inIgnoredContacts, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter, TempAllocator &inAllocator) const
+bool CharacterVirtual::GetFirstContactForSweep(RVec3Arg inPosition, Vec3Arg inDisplacement, Contact &outContact, const IgnoredContactList &inIgnoredContacts, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) const
 {
 	// Too small distance -> skip checking
 	float displacement_len_sq = inDisplacement.LengthSq();
@@ -224,29 +285,16 @@ bool CharacterVirtual::GetFirstContactForSweep(RVec3Arg inPosition, Vec3Arg inDi
 	settings.mReturnDeepestPoint = false;
 
 	// Cast shape
-	TempContactList contacts(inAllocator);
-	contacts.reserve(mMaxNumHits);
-	ContactCastCollector collector(mSystem, inDisplacement, mMaxNumHits, mUp, inIgnoredContacts, start.GetTranslation(), contacts);
+	Contact contact;
+	contact.mFraction = 1.0f + FLT_EPSILON;
+	ContactCastCollector collector(mSystem, this, inDisplacement, mUp, inIgnoredContacts, start.GetTranslation(), contact);
 	RShapeCast shape_cast(mShape, Vec3::sReplicate(1.0f), start, inDisplacement);
 	mSystem->GetNarrowPhaseQuery().CastShape(shape_cast, settings, start.GetTranslation(), collector, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter);
-	if (contacts.empty())
+	if (contact.mBodyB.IsInvalid())
 		return false;
 
-	// Sort the contacts on fraction
-	QuickSort(contacts.begin(), contacts.end(), [](const Contact &inLHS, const Contact &inRHS) { return inLHS.mFraction < inRHS.mFraction; });
-
-	// Check the first contact that will make us penetrate more than the allowed tolerance
-	bool valid_contact = false;
-	for (const Contact &c : contacts)
-		if (c.mDistance + c.mContactNormal.Dot(inDisplacement) < -mCollisionTolerance
-			&& ValidateContact(c))
-		{
-			outContact = c;
-			valid_contact = true;
-			break;
-		}
-	if (!valid_contact)
-		return false;
+	// Store contact
+	outContact = contact;
 
 	// Fetch the face we're colliding with
 	TransformedShape ts = mSystem->GetBodyInterface().GetTransformedShape(outContact.mBodyB);
@@ -878,7 +926,7 @@ void CharacterVirtual::MoveShape(RVec3 &ioPosition, Vec3Arg inVelocity, float in
 
 		// Do a sweep to test if the path is really unobstructed
 		Contact cast_contact;
-		if (GetFirstContactForSweep(ioPosition, displacement, cast_contact, ignored_contacts, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter, inAllocator))
+		if (GetFirstContactForSweep(ioPosition, displacement, cast_contact, ignored_contacts, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter))
 		{
 			displacement *= cast_contact.mFraction;
 			time_simulated *= cast_contact.mFraction;
@@ -1053,7 +1101,7 @@ bool CharacterVirtual::WalkStairs(float inDeltaTime, Vec3Arg inStepUp, Vec3Arg i
 	Vec3 up = inStepUp;
 	Contact contact;
 	IgnoredContactList dummy_ignored_contacts(inAllocator);
-	if (GetFirstContactForSweep(mPosition, up, contact, dummy_ignored_contacts, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter, inAllocator))
+	if (GetFirstContactForSweep(mPosition, up, contact, dummy_ignored_contacts, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter))
 	{
 		if (contact.mFraction < 1.0e-6f)
 			return false; // No movement, cancel
@@ -1086,7 +1134,7 @@ bool CharacterVirtual::WalkStairs(float inDeltaTime, Vec3Arg inStepUp, Vec3Arg i
 	// Note that we travel the same amount down as we travelled up with the character padding and the specified extra
 	// If we don't add the character padding, we may miss the floor (note that GetFirstContactForSweep will subtract the padding when it finds a hit)
 	Vec3 down = -up - mCharacterPadding * mUp + inStepDownExtra;
-	if (!GetFirstContactForSweep(new_position, down, contact, dummy_ignored_contacts, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter, inAllocator))
+	if (!GetFirstContactForSweep(new_position, down, contact, dummy_ignored_contacts, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter))
 		return false; // No floor found, we're in mid air, cancel stair walk
 
 #ifdef JPH_DEBUG_RENDERER
@@ -1124,7 +1172,7 @@ bool CharacterVirtual::WalkStairs(float inDeltaTime, Vec3Arg inStepUp, Vec3Arg i
 
 		// Then sweep down
 		Contact test_contact;
-		if (!GetFirstContactForSweep(test_position, down, test_contact, dummy_ignored_contacts, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter, inAllocator))
+		if (!GetFirstContactForSweep(test_position, down, test_contact, dummy_ignored_contacts, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter))
 			return false;
 
 	#ifdef JPH_DEBUG_RENDERER
@@ -1160,7 +1208,7 @@ bool CharacterVirtual::StickToFloor(Vec3Arg inStepDown, const BroadPhaseLayerFil
 	// Try to find the floor
 	Contact contact;
 	IgnoredContactList dummy_ignored_contacts(inAllocator);
-	if (!GetFirstContactForSweep(mPosition, inStepDown, contact, dummy_ignored_contacts, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter, inAllocator))
+	if (!GetFirstContactForSweep(mPosition, inStepDown, contact, dummy_ignored_contacts, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter))
 		return false; // If no floor found, don't update our position
 
 	// Calculate new position

Jolt/Physics/Character/CharacterVirtual.h

@@ -37,6 +37,7 @@ public:
 	float								mCollisionTolerance = 1.0e-3f;							///< How far we're willing to penetrate geometry
 	float								mCharacterPadding = 0.02f;								///< How far we try to stay away from the geometry, this ensures that the sweep will hit as little as possible lowering the collision cost and reducing the risk of getting stuck
 	uint								mMaxNumHits = 256;										///< Max num hits to collect in order to avoid excess of contact points collection
+	float								mHitReductionCosMaxAngle = 0.999f;						///< Cos(angle) where angle is the maximum angle between two hits contact normals that are allowed to be merged during hit reduction. Default is around 2.5 degrees. Set to -1 to turn off.
 	float								mPenetrationRecoverySpeed = 1.0f;						///< This value governs how fast a penetration will be resolved, 0 = nothing is resolved, 1 = everything in one update
 };
 
@@ -140,6 +141,16 @@ public:
 	uint								GetMaxNumHits() const									{ return mMaxNumHits; }
 	void								SetMaxNumHits(uint inMaxHits)							{ mMaxNumHits = inMaxHits; }
 
+	/// Cos(angle) where angle is the maximum angle between two hits contact normals that are allowed to be merged during hit reduction. Default is around 2.5 degrees. Set to -1 to turn off.
+	float								GetHitReductionCosMaxAngle() const						{ return mHitReductionCosMaxAngle; }
+	void								SetHitReductionCosMaxAngle(float inCosMaxAngle)			{ mHitReductionCosMaxAngle = inCosMaxAngle; }
+
+	/// Returns if we exceeded the maximum number of hits during the last collision check and had to discard hits based on distance.
+	/// This can be used to find areas that have too complex geometry for the character to navigate properly.
+	/// To solve you can either increase the max number of hits or simplify the geometry. Note that the character simulation will
+	/// try to do its best to select the most relevant contacts to avoid the character from getting stuck.
+	bool								GetMaxHitsExceeded() const								{ return mMaxHitsExceeded; }
+
 	/// An extra offset applied to the shape in local space. This allows applying an extra offset to the shape in local space. Note that setting it on the fly can cause the shape to teleport into collision.
 	Vec3								GetShapeOffset() const									{ return mShapeOffset; }
 	void								SetShapeOffset(Vec3Arg inShapeOffset)					{ mShapeOffset = inShapeOffset; }
@@ -310,7 +321,7 @@ private:
 	class ContactCollector : public CollideShapeCollector
 	{
 	public:
-										ContactCollector(PhysicsSystem *inSystem, uint inMaxHits, Vec3Arg inUp, RVec3Arg inBaseOffset, TempContactList &outContacts) : mBaseOffset(inBaseOffset), mUp(inUp), mSystem(inSystem), mContacts(outContacts), mMaxHits(inMaxHits) { }
+										ContactCollector(PhysicsSystem *inSystem, uint inMaxHits, float inHitReductionCosMaxAngle, Vec3Arg inUp, RVec3Arg inBaseOffset, TempContactList &outContacts) : mBaseOffset(inBaseOffset), mUp(inUp), mSystem(inSystem), mContacts(outContacts), mMaxHits(inMaxHits), mHitReductionCosMaxAngle(inHitReductionCosMaxAngle) { }
 
 		virtual void					AddHit(const CollideShapeResult &inResult) override;
 
@@ -319,13 +330,15 @@ private:
 		PhysicsSystem *					mSystem;
 		TempContactList &				mContacts;
 		uint							mMaxHits;
+		float							mHitReductionCosMaxAngle;
+		bool							mMaxHitsExceeded = false;
 	};
 
 	// A collision collector that collects hits for CastShape
 	class ContactCastCollector : public CastShapeCollector
 	{
 	public:
-										ContactCastCollector(PhysicsSystem *inSystem, Vec3Arg inDisplacement, uint inMaxHits, Vec3Arg inUp, const IgnoredContactList &inIgnoredContacts, RVec3Arg inBaseOffset, TempContactList &outContacts) : mBaseOffset(inBaseOffset), mDisplacement(inDisplacement), mUp(inUp), mSystem(inSystem), mIgnoredContacts(inIgnoredContacts), mContacts(outContacts), mMaxHits(inMaxHits) { }
+										ContactCastCollector(PhysicsSystem *inSystem, const CharacterVirtual *inCharacter, Vec3Arg inDisplacement, Vec3Arg inUp, const IgnoredContactList &inIgnoredContacts, RVec3Arg inBaseOffset, Contact &outContact) : mBaseOffset(inBaseOffset), mDisplacement(inDisplacement), mUp(inUp), mSystem(inSystem), mCharacter(inCharacter), mIgnoredContacts(inIgnoredContacts), mContact(outContact) { }
 
 		virtual void					AddHit(const ShapeCastResult &inResult) override;
 
@@ -333,9 +346,9 @@ private:
 		Vec3							mDisplacement;
 		Vec3							mUp;
 		PhysicsSystem *					mSystem;
+		const CharacterVirtual *		mCharacter;
 		const IgnoredContactList &		mIgnoredContacts;
-		TempContactList &				mContacts;
-		uint							mMaxHits;
+		Contact &						mContact;
 	};
 
 	// Helper function to convert a Jolt collision result into a contact
@@ -372,7 +385,7 @@ private:
 	bool								HandleContact(Vec3Arg inVelocity, Constraint &ioConstraint, float inDeltaTime) const;
 
 	// Does a swept test of the shape from inPosition with displacement inDisplacement, returns true if there was a collision
-	bool								GetFirstContactForSweep(RVec3Arg inPosition, Vec3Arg inDisplacement, Contact &outContact, const IgnoredContactList &inIgnoredContacts, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter, TempAllocator &inAllocator) const;
+	bool								GetFirstContactForSweep(RVec3Arg inPosition, Vec3Arg inDisplacement, Contact &outContact, const IgnoredContactList &inIgnoredContacts, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) const;
 
 	// Store contacts so that we have proper ground information
 	void								StoreActiveContacts(const TempContactList &inContacts, TempAllocator &inAllocator);
@@ -400,6 +413,7 @@ private:
 	float								mCollisionTolerance;									// How far we're willing to penetrate geometry
 	float								mCharacterPadding;										// How far we try to stay away from the geometry, this ensures that the sweep will hit as little as possible lowering the collision cost and reducing the risk of getting stuck
 	uint								mMaxNumHits;											// Max num hits to collect in order to avoid excess of contact points collection
+	float								mHitReductionCosMaxAngle;								// Cos(angle) where angle is the maximum angle between two hits contact normals that are allowed to be merged during hit reduction. Default is around 2.5 degrees. Set to -1 to turn off.
 	float								mPenetrationRecoverySpeed;								// This value governs how fast a penetration will be resolved, 0 = nothing is resolved, 1 = everything in one update
 
 	// Character mass (kg)
@@ -425,6 +439,9 @@ private:
 
 	// Remembers the delta time of the last update
 	float								mLastDeltaTime = 1.0f / 60.0f;
+
+	// Remember if we exceeded the maximum number of hits and had to remove similar contacts
+	mutable bool						mMaxHitsExceeded = false;
 };
 
 JPH_NAMESPACE_END

Samples/Tests/Character/CharacterBaseTest.cpp

@@ -68,6 +68,7 @@ static const float cMeshWallWidth = 2.0f;
 static const float cMeshWallStepStart = 0.5f;
 static const float cMeshWallStepEnd = 4.0f;
 static const int cMeshWallSegments = 25;
+static const RVec3 cHalfCylinderPosition(5.0f, 0, 8.0f);
 
 void CharacterBaseTest::Initialize()
 {
@@ -361,6 +362,50 @@ void CharacterBaseTest::Initialize()
 			BodyCreationSettings wall(&mesh, cMeshWallPosition, Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING);
 			mBodyInterface->CreateAndAddBody(wall, EActivation::DontActivate);
 		}
+
+		// Create a half cylinder with caps for testing contact point limit
+		{
+			VertexList vertices;
+			IndexedTriangleList triangles;
+
+			// The half cylinder
+			const int cPosSegments = 2;
+			const int cAngleSegments = 512;
+			const float cCylinderLength = 2.0f;
+			for (int pos = 0; pos < cPosSegments; ++pos)
+				for (int angle = 0; angle < cAngleSegments; ++angle)
+				{
+					uint32 start = (uint32)vertices.size();
+
+					float radius = cCharacterRadiusStanding + 0.05f;
+					float angle_rad = (-0.5f + float(angle) / cAngleSegments) * JPH_PI;
+					float s = Sin(angle_rad);
+					float c = Cos(angle_rad);
+					float x = cCylinderLength * (-0.5f + float(pos) / (cPosSegments - 1));
+					float y = angle == 0 || angle == cAngleSegments - 1? 0.5f : (1.0f - c) * radius;
+					float z = s * radius;
+					vertices.push_back(Float3(x, y, z));
+
+					if (pos > 0 && angle > 0)
+					{
+						triangles.push_back(IndexedTriangle(start, start - 1, start - cAngleSegments));
+						triangles.push_back(IndexedTriangle(start - 1, start - cAngleSegments - 1, start - cAngleSegments));
+					}
+				}
+
+			// Add end caps
+			uint32 end = cAngleSegments * (cPosSegments - 1);
+			for (int angle = 0; angle < cAngleSegments - 1; ++angle)
+			{
+				triangles.push_back(IndexedTriangle(0, angle + 1, angle));
+				triangles.push_back(IndexedTriangle(end, end + angle, end + angle + 1));
+			}
+
+			MeshShapeSettings mesh(vertices, triangles);
+			mesh.SetEmbedded();
+			BodyCreationSettings mesh_cylinder(&mesh, cHalfCylinderPosition, Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING);
+			mBodyInterface->CreateAndAddBody(mesh_cylinder, EActivation::DontActivate);
+		}
 	}
 	else
 	{

UnitTests/Physics/CharacterVirtualTests.cpp

@@ -487,4 +487,130 @@ TEST_SUITE("CharacterVirtualTests")
 			CHECK_APPROX_EQUAL(character.mCharacter->GetPosition(), expected_position, 1.0e-2f);
 		}
 	}
+
+	TEST_CASE("TestContactPointLimit")
+	{
+		PhysicsTestContext ctx;
+		Body &floor = ctx.CreateFloor();
+
+		// Create character at the origin
+		Character character(ctx);
+		character.mInitialPosition = RVec3(0, 1, 0);
+		character.mUpdateSettings.mStickToFloorStepDown = Vec3::sZero();
+		character.mUpdateSettings.mWalkStairsStepUp = Vec3::sZero();
+		character.Create();
+
+		// Radius including pading
+		const float character_radius = character.mRadiusStanding + character.mCharacterSettings.mCharacterPadding;
+
+		// Create a half cylinder with caps for testing contact point limit
+		VertexList vertices;
+		IndexedTriangleList triangles;
+
+		// The half cylinder
+		const int cPosSegments = 2;
+		const int cAngleSegments = 768;
+		const float cCylinderLength = 2.0f;
+		for (int pos = 0; pos < cPosSegments; ++pos)
+			for (int angle = 0; angle < cAngleSegments; ++angle)
+			{
+				uint32 start = (uint32)vertices.size();
+
+				float radius = character_radius + 0.01f;
+				float angle_rad = (-0.5f + float(angle) / cAngleSegments) * JPH_PI;
+				float s = Sin(angle_rad);
+				float c = Cos(angle_rad);
+				float x = cCylinderLength * (-0.5f + float(pos) / (cPosSegments - 1));
+				float y = angle == 0 || angle == cAngleSegments - 1? 0.5f : (1.0f - c) * radius;
+				float z = s * radius;
+				vertices.push_back(Float3(x, y, z));
+
+				if (pos > 0 && angle > 0)
+				{
+					triangles.push_back(IndexedTriangle(start, start - 1, start - cAngleSegments));
+					triangles.push_back(IndexedTriangle(start - 1, start - cAngleSegments - 1, start - cAngleSegments));
+				}
+			}
+
+		// Add end caps
+		uint32 end = cAngleSegments * (cPosSegments - 1);
+		for (int angle = 0; angle < cAngleSegments - 1; ++angle)
+		{
+			triangles.push_back(IndexedTriangle(0, angle + 1, angle));
+			triangles.push_back(IndexedTriangle(end, end + angle, end + angle + 1));
+		}
+
+		// Create test body
+		MeshShapeSettings mesh(vertices, triangles);
+		mesh.SetEmbedded();
+		BodyCreationSettings mesh_cylinder(&mesh, character.mInitialPosition, Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING);
+		BodyID cylinder_id = ctx.GetBodyInterface().CreateAndAddBody(mesh_cylinder, EActivation::DontActivate);
+
+		// End positions that can be reached by character
+		RVec3 pos_end(0.5_r * cCylinderLength - character_radius, 1, 0);
+		RVec3 neg_end(-0.5_r * cCylinderLength + character_radius, 1, 0);
+
+		// Move towards positive cap and test if we hit the end
+		character.mHorizontalSpeed = Vec3(cCylinderLength, 0, 0);
+		for (int t = 0; t < 60; ++t)
+		{
+			character.Step();
+			CHECK(character.mCharacter->GetMaxHitsExceeded());
+			CHECK(character.mCharacter->GetActiveContacts().size() < character.mCharacter->GetMaxNumHits());
+			CHECK(character.mCharacter->GetGroundBodyID() == cylinder_id);
+			CHECK(character.mCharacter->GetGroundNormal().Dot(Vec3::sAxisY()) > 0.999f);
+		}
+		CHECK_APPROX_EQUAL(character.mCharacter->GetPosition(), pos_end, 1.0e-4f);
+
+		// Move towards negative cap and test if we hit the end
+		character.mHorizontalSpeed = Vec3(-cCylinderLength, 0, 0);
+		for (int t = 0; t < 60; ++t)
+		{
+			character.Step();
+			CHECK(character.mCharacter->GetMaxHitsExceeded());
+			CHECK(character.mCharacter->GetActiveContacts().size() < character.mCharacter->GetMaxNumHits());
+			CHECK(character.mCharacter->GetGroundBodyID() == cylinder_id);
+			CHECK(character.mCharacter->GetGroundNormal().Dot(Vec3::sAxisY()) > 0.999f);
+		}
+		CHECK_APPROX_EQUAL(character.mCharacter->GetPosition(), neg_end, 1.0e-4f);
+
+		// Turn off contact point reduction
+		character.mCharacter->SetHitReductionCosMaxAngle(-1.0f);
+
+		// Move towards positive cap and test that we did not reach the end
+		character.mHorizontalSpeed = Vec3(cCylinderLength, 0, 0);
+		for (int t = 0; t < 60; ++t)
+		{
+			character.Step();
+			CHECK(character.mCharacter->GetMaxHitsExceeded());
+			CHECK(character.mCharacter->GetActiveContacts().size() == character.mCharacter->GetMaxNumHits());
+		}
+		RVec3 cur_pos = character.mCharacter->GetPosition();
+		CHECK((pos_end - cur_pos).Length() > 0.01_r);
+
+		// Move towards negative cap and test that we got stuck
+		character.mHorizontalSpeed = Vec3(-cCylinderLength, 0, 0);
+		for (int t = 0; t < 60; ++t)
+		{
+			character.Step();
+			CHECK(character.mCharacter->GetMaxHitsExceeded());
+			CHECK(character.mCharacter->GetActiveContacts().size() == character.mCharacter->GetMaxNumHits());
+		}
+		CHECK(cur_pos.IsClose(character.mCharacter->GetPosition(), 1.0e-6f));
+
+		// Now teleport the character next to the half cylinder
+		character.mCharacter->SetPosition(RVec3(0, 0, 1));
+
+		// Move in positive X and check that we did not exceed max hits and that we were able to move unimpeded
+		character.mHorizontalSpeed = Vec3(cCylinderLength, 0, 0);
+		for (int t = 0; t < 60; ++t)
+		{
+			character.Step();
+			CHECK(!character.mCharacter->GetMaxHitsExceeded());
+			CHECK(character.mCharacter->GetActiveContacts().size() == 1); // We should only hit the floor
+			CHECK(character.mCharacter->GetGroundBodyID() == floor.GetID());
+			CHECK(character.mCharacter->GetGroundNormal().Dot(Vec3::sAxisY()) > 0.999f);
+		}
+		CHECK_APPROX_EQUAL(character.mCharacter->GetPosition(), RVec3(cCylinderLength, 0, 1), 1.0e-4f);
+	}
 }