Implemented a function to do stair walking for the virtual character (#160)

Jolt/Physics/Character/CharacterVirtual.cpp

@@ -820,6 +820,116 @@ bool CharacterVirtual::SetShape(const Shape *inShape, float inMaxPenetrationDept
 	return mShape == inShape;
 }
 
+bool CharacterVirtual::CanWalkStairs() const
+{
+	// Check if there's enough horizontal velocity to trigger a stair walk
+	Vec3 horizontal_velocity = mLinearVelocity - mLinearVelocity.Dot(mUp) * mUp;
+	if (horizontal_velocity.IsNearZero(1.0e-6f))
+		return false;
+
+	// Check contacts for steep slopes
+	for (const Contact &c : mActiveContacts)
+		if (c.mHadCollision
+			&& c.mNormal.Dot(horizontal_velocity - c.mLinearVelocity) < 0.0f // Pushing into the contact
+			&& c.mNormal.Dot(mUp) < mCosMaxSlopeAngle) // Slope too steep
+			return true;
+
+	return false;
+}
+
+bool CharacterVirtual::WalkStairs(float inDeltaTime, Vec3Arg inGravity, Vec3Arg inStepUp, Vec3Arg inStepForward, Vec3Arg inStepForwardTest, Vec3Arg inStepDownExtra, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, TempAllocator &inAllocator)
+{
+	// Move up
+	Vec3 up = inStepUp;
+	Contact contact;
+	IgnoredContactList dummy_ignored_contacts(inAllocator);
+	if (GetFirstContactForSweep(mPosition, up, contact, dummy_ignored_contacts, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inAllocator))
+	{
+		if (contact.mFraction < 1.0e-6f)
+			return false; // No movement, cancel
+
+		// Limit up movement to the first contact point
+		up *= contact.mFraction;
+	}
+	Vec3 up_position = mPosition + up;
+
+#ifdef JPH_DEBUG_RENDERER
+	// Draw sweep up
+	if (sDrawWalkStairs)
+		DebugRenderer::sInstance->DrawArrow(mPosition, up_position, Color::sGrey, 0.01f);
+#endif // JPH_DEBUG_RENDERER
+
+	// Horizontal movement
+	Vec3 new_position = up_position;
+	MoveShape(new_position, inStepForward / inDeltaTime, inGravity, inDeltaTime, nullptr, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inAllocator);
+	if (new_position.IsClose(up_position, 1.0e-8f))
+		return false; // No movement, cancel
+
+#ifdef JPH_DEBUG_RENDERER
+	// Draw horizontal sweep
+	if (sDrawWalkStairs)
+		DebugRenderer::sInstance->DrawArrow(up_position, new_position, Color::sGrey, 0.01f);
+#endif // JPH_DEBUG_RENDERER
+
+	// Move down towards the floor.
+	// Note that we travel the same amount down as we travelled up with the specified extra
+	Vec3 down = -up + inStepDownExtra;
+	if (!GetFirstContactForSweep(new_position, down, contact, dummy_ignored_contacts, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inAllocator))
+		return false; // No floor found, we're in mid air, cancel stair walk
+
+#ifdef JPH_DEBUG_RENDERER
+	// Draw sweep down
+	if (sDrawWalkStairs)
+	{
+		Vec3 debug_pos = new_position + contact.mFraction * down; 
+		DebugRenderer::sInstance->DrawArrow(new_position, debug_pos, Color::sYellow, 0.01f);
+		DebugRenderer::sInstance->DrawArrow(contact.mPosition, contact.mPosition + contact.mNormal, Color::sYellow, 0.01f);
+		mShape->Draw(DebugRenderer::sInstance, GetCenterOfMassTransform(debug_pos, mRotation, mShape), Vec3::sReplicate(1.0f), Color::sYellow, false, true);
+	}
+#endif // JPH_DEBUG_RENDERER
+
+	// Test for floor that will support the character
+	if (mCosMaxSlopeAngle < 0.999f // If cos(slope angle) is close to 1 then there's no limit
+		&& contact.mNormal.Dot(mUp) < mCosMaxSlopeAngle) // Check slope angle
+	{
+		// If no test position was provided, we cancel the stair walk
+		if (inStepForwardTest.IsNearZero())
+			return false;
+
+		// Delta time may be very small, so it may be that we hit the edge of a step and the normal is too horizontal.
+		// In order to judge if the floor is flat further along the sweep, we test again for a floor at inStepForwardTest
+		// and check if the normal is valid there.
+		Vec3 test_position = up_position + inStepForwardTest;
+		Contact test_contact;
+		bool hit = GetFirstContactForSweep(test_position, down, test_contact, dummy_ignored_contacts, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inAllocator);
+		if (!hit)
+			return false;
+
+	#ifdef JPH_DEBUG_RENDERER
+		// Draw 2nd sweep down
+		if (sDrawWalkStairs)
+		{
+			Vec3 debug_pos = test_position + test_contact.mFraction * down; 
+			DebugRenderer::sInstance->DrawArrow(test_position, debug_pos, Color::sCyan, 0.01f);
+			DebugRenderer::sInstance->DrawArrow(test_contact.mPosition, test_contact.mPosition + test_contact.mNormal, Color::sCyan, 0.01f);
+			mShape->Draw(DebugRenderer::sInstance, GetCenterOfMassTransform(debug_pos, mRotation, mShape), Vec3::sReplicate(1.0f), Color::sCyan, false, true);
+		}
+	#endif // JPH_DEBUG_RENDERER
+
+		if (test_contact.mNormal.Dot(mUp) < mCosMaxSlopeAngle)
+			return false;
+	}
+
+	// Calculate new down position
+	down *= contact.mFraction;
+	new_position += down;
+
+	// Move the character to the new location
+	SetPosition(new_position);
+	RefreshContacts(inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inAllocator);
+	return true;
+}
+
 void CharacterVirtual::SaveState(StateRecorder &inStream) const
 {
 	CharacterBase::SaveState(inStream);

Jolt/Physics/Character/CharacterVirtual.h

@@ -123,6 +123,24 @@ public:
 	/// @param inAllocator An allocator for temporary allocations. All memory will be freed by the time this function returns.
 	void								Update(float inDeltaTime, Vec3Arg inGravity, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, TempAllocator &inAllocator);
 
+	/// This function will return true if the character has moved into a slope that is too steep (e.g. a vertical wall).
+	/// You would call WalkStairs to attempt to step up stairs.
+	bool								CanWalkStairs() const;
+
+	/// When stair walking is needed, you can call the WalkStairs function to cast up, forward and down again to try to find a valid position
+	/// @param inDeltaTime Time step to simulate.
+	/// @param inGravity Gravity vector (m/s^2)
+	/// @param inStepUp The direction and distance to step up (this corresponds to the max step height)
+	/// @param inStepForward The direction and distance to step forward after the step up
+	/// @param inStepForwardTest When running at a high frequency, inStepForward can be very small and it's likely that you hit the side of the stairs on the way down. This could produce a normal that violates the max slope angle. If this happens, we test again using this distance from the up position to see if we find a valid slope.
+	/// @param inStepDownExtra An additional translation that is added when stepping down at the end. Allows you to step further down than up. Set to zero if you don't want this.
+	/// @param inBroadPhaseLayerFilter Filter that is used to check if the character collides with something in the broadphase.
+	/// @param inObjectLayerFilter Filter that is used to check if a character collides with a layer.
+	/// @param inBodyFilter Filter that is used to check if a character collides with a body.
+	/// @param inAllocator An allocator for temporary allocations. All memory will be freed by the time this function returns.
+	/// @return true if the stair walk was successful
+	bool								WalkStairs(float inDeltaTime, Vec3Arg inGravity, Vec3Arg inStepUp, Vec3Arg inStepForward, Vec3Arg inStepForwardTest, Vec3Arg inStepDownExtra, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, TempAllocator &inAllocator);
+
 	/// This function can be used after a character has teleported to determine the new contacts with the world.
 	void								RefreshContacts(const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, TempAllocator &inAllocator);
 
@@ -154,6 +172,7 @@ public:
 
 #ifdef JPH_DEBUG_RENDERER
 	static inline bool					sDrawConstraints = false;								///< Draw the current state of the constraints for iteration 0 when creating them
+	static inline bool					sDrawWalkStairs = false;								///< Draw the state of the walk stairs algorithm
 #endif
 
 private:

Samples/SamplesApp.cpp

@@ -396,6 +396,7 @@ SamplesApp::SamplesApp()
 		mDebugUI->CreateCheckBox(drawing_options, "Draw Height Field Shape Triangle Outlines", HeightFieldShape::sDrawTriangleOutlines, [](UICheckBox::EState inState) { HeightFieldShape::sDrawTriangleOutlines = inState == UICheckBox::STATE_CHECKED; });
 		mDebugUI->CreateCheckBox(drawing_options, "Draw Submerged Volumes", Shape::sDrawSubmergedVolumes, [](UICheckBox::EState inState) { Shape::sDrawSubmergedVolumes = inState == UICheckBox::STATE_CHECKED; });
 		mDebugUI->CreateCheckBox(drawing_options, "Draw Character Virtual Constraints", CharacterVirtual::sDrawConstraints, [](UICheckBox::EState inState) { CharacterVirtual::sDrawConstraints = inState == UICheckBox::STATE_CHECKED; });
+		mDebugUI->CreateCheckBox(drawing_options, "Draw Character Virtual Walk Stairs", CharacterVirtual::sDrawWalkStairs, [](UICheckBox::EState inState) { CharacterVirtual::sDrawWalkStairs = inState == UICheckBox::STATE_CHECKED; });
 		mDebugUI->ShowMenu(drawing_options);
 	});
 #endif // JPH_DEBUG_RENDERER

Samples/Tests/Character/CharacterBaseTest.cpp

@@ -31,7 +31,7 @@ const char *CharacterBaseTest::sScenes[] =
 const char *CharacterBaseTest::sSceneName = "ObstacleCourse";
 
 // Scene constants
-static const Vec3 cRotatingPosition(-5, 0.25f, 15);
+static const Vec3 cRotatingPosition(-5, 0.15f, 15);
 static const Quat cRotatingOrientation = Quat::sIdentity();
 static const Vec3 cVerticallyMovingPosition(0, 2.0f, 15);
 static const Quat cVerticallyMovingOrientation = Quat::sIdentity();
@@ -42,6 +42,12 @@ static const Quat cRampOrientation = Quat::sRotation(Vec3::sAxisX(), -0.25f * JP
 static const Vec3 cRampBlocksStart = cRampPosition + Vec3(-3.0f, 3.0f, 1.5f);
 static const Vec3 cRampBlocksDelta = Vec3(2.0f, 0, 0);
 static const float cRampBlocksTime = 5.0f;
+static const Vec3 cBumpsPosition = Vec3(-5.0f, 0, 2.5f);
+static const float cBumpHeight = 0.05f;
+static const float cBumpWidth = 0.01f;
+static const float cBumpDelta = 0.5f;
+static const Vec3 cStairsPosition = Vec3(-10.0f, 0, 2.5f);
+static const float cStairsStepHeight = 0.3f;
 
 void CharacterBaseTest::Initialize()
 {
@@ -77,7 +83,7 @@ void CharacterBaseTest::Initialize()
 
 		{
 			// Kinematic blocks to test interacting with moving objects
-			Ref<Shape> kinematic = new BoxShape(Vec3(1, 0.25f, 3.0f));
+			Ref<Shape> kinematic = new BoxShape(Vec3(1, 0.15f, 3.0f));
 			mRotatingBody = mBodyInterface->CreateAndAddBody(BodyCreationSettings(kinematic, cRotatingPosition, cRotatingOrientation, EMotionType::Kinematic, Layers::MOVING), EActivation::Activate);
 			mVerticallyMovingBody = mBodyInterface->CreateAndAddBody(BodyCreationSettings(kinematic, cVerticallyMovingPosition, cVerticallyMovingOrientation, EMotionType::Kinematic, Layers::MOVING), EActivation::Activate);
 			mHorizontallyMovingBody = mBodyInterface->CreateAndAddBody(BodyCreationSettings(kinematic, cHorizontallyMovingPosition, cHorizontallyMovingOrientation, EMotionType::Kinematic, Layers::MOVING), EActivation::Activate);
@@ -129,6 +135,26 @@ void CharacterBaseTest::Initialize()
 			Quat rotation = Quat::sRotation(Vec3::sAxisY(), 0.5f * JPH_PI * i);
 			mBodyInterface->CreateAndAddBody(BodyCreationSettings(funnel, Vec3(10.0f, 0.1f, 5.0f) + rotation * Vec3(0.2f, 0, 0), rotation * Quat::sRotation(Vec3::sAxisZ(), -DegreesToRadians(40.0f)), EMotionType::Static, Layers::NON_MOVING), EActivation::DontActivate);
 		}
+
+		// Create small bumps
+		{
+			BodyCreationSettings step(new BoxShape(Vec3(2.0f, 0.5f * cBumpHeight, 0.5f * cBumpWidth), 0.0f), Vec3::sZero(), Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING);
+			for (int i = 0; i < 10; ++i)
+			{
+				step.mPosition = cBumpsPosition + Vec3(0, 0.5f * cBumpHeight, cBumpDelta * i);
+				mBodyInterface->CreateAndAddBody(step, EActivation::DontActivate);
+			}
+		}
+
+		// Create stairs
+		{
+			BodyCreationSettings step(new BoxShape(Vec3(2.0f, 0.5f * cStairsStepHeight, 0.5f * cStairsStepHeight)), Vec3::sZero(), Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING);
+			for (int i = 0; i < 10; ++i)
+			{
+				step.mPosition = cStairsPosition + Vec3(0, cStairsStepHeight * (0.5f + i), cStairsStepHeight * i);
+				mBodyInterface->CreateAndAddBody(step, EActivation::DontActivate);
+			}
+		}
 	}
 	else
 	{

Samples/Tests/Character/CharacterVirtualTest.cpp

@@ -15,6 +15,9 @@ JPH_IMPLEMENT_RTTI_VIRTUAL(CharacterVirtualTest)
 	JPH_ADD_BASE_CLASS(CharacterVirtualTest, CharacterBaseTest)
 }
 
+static const Vec3 cStepUpHeight = Vec3(0.0f, 0.4f, 0.0f);
+static const float cMinStepForward = 0.15f;
+
 void CharacterVirtualTest::Initialize()
 {
 	CharacterBaseTest::Initialize();
@@ -54,6 +57,34 @@ void CharacterVirtualTest::PrePhysicsUpdate(const PreUpdateParams &inParams)
 	// Update the character position (instant, do not have to wait for physics update)
 	mCharacter->Update(inParams.mDeltaTime, mPhysicsSystem->GetGravity(), mPhysicsSystem->GetDefaultBroadPhaseLayerFilter(Layers::MOVING), mPhysicsSystem->GetDefaultLayerFilter(Layers::MOVING), { }, *mTempAllocator);
 
+	// Allow user to turn off walk stairs algorithm
+	if (sEnableWalkStairs)
+	{
+		// Calculate how much we wanted to move horizontally
+		Vec3 desired_horizontal_step = mCharacter->GetLinearVelocity() * inParams.mDeltaTime;
+		desired_horizontal_step.SetY(0);
+		float desired_horizontal_step_len = desired_horizontal_step.Length();
+
+		// Calculate how much we moved horizontally
+		Vec3 achieved_horizontal_step = mCharacter->GetPosition() - old_position;
+		achieved_horizontal_step.SetY(0);
+		float achieved_horizontal_step_len = achieved_horizontal_step.Length();
+
+		// If we didn't move as far as we wanted and we're against a slope that's too steep
+		if (achieved_horizontal_step_len + 1.0e-4f < desired_horizontal_step_len
+			&& mCharacter->CanWalkStairs())
+		{
+			// Calculate how much we should step forward
+			Vec3 step_forward_normalized = desired_horizontal_step / desired_horizontal_step_len;
+			Vec3 step_forward = step_forward_normalized * (desired_horizontal_step_len - achieved_horizontal_step_len);
+
+			// Calculate how far to scan ahead for a floor
+			Vec3 step_forward_test = step_forward_normalized * cMinStepForward;
+
+			mCharacter->WalkStairs(inParams.mDeltaTime, mPhysicsSystem->GetGravity(), cStepUpHeight, step_forward, step_forward_test, Vec3::sZero(), mPhysicsSystem->GetDefaultBroadPhaseLayerFilter(Layers::MOVING), mPhysicsSystem->GetDefaultLayerFilter(Layers::MOVING), { }, *mTempAllocator);
+		}
+	}
+
 	// Calculate effective velocity
 	Vec3 new_position = mCharacter->GetPosition();
 	Vec3 velocity = (new_position - old_position) / inParams.mDeltaTime;
@@ -121,11 +152,12 @@ void CharacterVirtualTest::CreateSettingsMenu(DebugUI *inUI, UIElement *inSubMen
 	inUI->CreateTextButton(inSubMenu, "Configuration Settings", [=]() {
 		UIElement *configuration_settings = inUI->CreateMenu();
 		
-		inUI->CreateSlider(configuration_settings, "Max Slope Angle (degrees)", RadiansToDegrees(sMaxSlopeAngle), 0.0f, 90.0f, 1.0f, [=](float inValue) { sMaxSlopeAngle = DegreesToRadians(inValue); });
-		inUI->CreateSlider(configuration_settings, "Max Strength (N)", sMaxStrength, 0.0f, 500.0f, 1.0f, [=](float inValue) { sMaxStrength = inValue; });
-		inUI->CreateSlider(configuration_settings, "Character Padding", sCharacterPadding, 0.01f, 0.5f, 0.01f, [=](float inValue) { sCharacterPadding = inValue; });
-		inUI->CreateSlider(configuration_settings, "Penetration Recovery Speed", sPenetrationRecoverySpeed, 0.0f, 1.0f, 0.05f, [=](float inValue) { sPenetrationRecoverySpeed = inValue; });
-		inUI->CreateSlider(configuration_settings, "Predictive Contact Distance", sPredictiveContactDistance, 0.01f, 1.0f, 0.01f, [=](float inValue) { sPredictiveContactDistance = inValue; });
+		inUI->CreateSlider(configuration_settings, "Max Slope Angle (degrees)", RadiansToDegrees(sMaxSlopeAngle), 0.0f, 90.0f, 1.0f, [](float inValue) { sMaxSlopeAngle = DegreesToRadians(inValue); });
+		inUI->CreateSlider(configuration_settings, "Max Strength (N)", sMaxStrength, 0.0f, 500.0f, 1.0f, [](float inValue) { sMaxStrength = inValue; });
+		inUI->CreateSlider(configuration_settings, "Character Padding", sCharacterPadding, 0.01f, 0.5f, 0.01f, [](float inValue) { sCharacterPadding = inValue; });
+		inUI->CreateSlider(configuration_settings, "Penetration Recovery Speed", sPenetrationRecoverySpeed, 0.0f, 1.0f, 0.05f, [](float inValue) { sPenetrationRecoverySpeed = inValue; });
+		inUI->CreateSlider(configuration_settings, "Predictive Contact Distance", sPredictiveContactDistance, 0.01f, 1.0f, 0.01f, [](float inValue) { sPredictiveContactDistance = inValue; });
+		inUI->CreateCheckBox(configuration_settings, "Enable Walk Stairs", sEnableWalkStairs, [](UICheckBox::EState inState) { sEnableWalkStairs = inState == UICheckBox::STATE_CHECKED; });
 		inUI->CreateTextButton(configuration_settings, "Accept Changes", [=]() { RestartTest(); });
 		inUI->ShowMenu(configuration_settings);
 	});

Samples/Tests/Character/CharacterVirtualTest.h

@@ -42,6 +42,7 @@ private:
 	static inline float		sCharacterPadding = 0.02f;
 	static inline float		sPenetrationRecoverySpeed = 1.0f;
 	static inline float		sPredictiveContactDistance = 0.1f;
+	static inline bool		sEnableWalkStairs = true;
 
 	// The 'player' character
 	Ref<CharacterVirtual>	mCharacter;