Character improvements (#231)

- Use surface instead of contact normal to check if slope is too steep
- Reduce amount of jittering by keeping track of last velocity applied and stopping sliding when velocity reverses
- Fixed bug in stair walk: 2nd sweep wasn't doing a horizontal sweep first which could cause it to end up in the middle of something at which point the sweep down would completely ignore this (because initial overlaps are ignored)
- Drawing contact point more subtle
- Draw constraints only in the first iteration (else they will start to overlap and become unreadable)

Jolt/Physics/Character/CharacterVirtual.cpp

@@ -34,11 +34,16 @@ CharacterVirtual::CharacterVirtual(const CharacterVirtualSettings *inSettings, V
 }
 
 template <class taCollector>
-void CharacterVirtual::sFillContactProperties(Contact &outContact, const Body &inBody, const taCollector &inCollector, const CollideShapeResult &inResult)
+void CharacterVirtual::sFillContactProperties(Contact &outContact, const Body &inBody, Vec3Arg inUp, const taCollector &inCollector, const CollideShapeResult &inResult)
 {
 	outContact.mPosition = inResult.mContactPointOn2;	
 	outContact.mLinearVelocity = inBody.GetPointVelocity(inResult.mContactPointOn2);
-	outContact.mNormal = -inResult.mPenetrationAxis.NormalizedOr(Vec3::sZero());
+	outContact.mContactNormal = -inResult.mPenetrationAxis.NormalizedOr(Vec3::sZero());
+	outContact.mSurfaceNormal = inCollector.GetContext()->GetWorldSpaceSurfaceNormal(inResult.mSubShapeID2, inResult.mContactPointOn2);
+	if (outContact.mContactNormal.Dot(outContact.mSurfaceNormal) < 0.0f)
+		outContact.mSurfaceNormal = -outContact.mSurfaceNormal; // Flip surface normal if we're hitting a back face
+	if (outContact.mContactNormal.Dot(inUp) > outContact.mSurfaceNormal.Dot(inUp))
+		outContact.mSurfaceNormal = outContact.mContactNormal; // Replace surface normal with contact normal if the contact normal is pointing more upwards
 	outContact.mDistance = -inResult.mPenetrationDepth;
 	outContact.mBodyB = inResult.mBodyID2;
 	outContact.mSubShapeIDB = inResult.mSubShapeID2;
@@ -56,7 +61,7 @@ void CharacterVirtual::ContactCollector::AddHit(const CollideShapeResult &inResu
 
 		mContacts.emplace_back();
 		Contact &contact = mContacts.back();
-		sFillContactProperties(contact, body, *this, inResult);
+		sFillContactProperties(contact, body, mUp, *this, inResult);
 		contact.mFraction = 0.0f;
 
 		// Protection from excess of contact points
@@ -82,7 +87,7 @@ void CharacterVirtual::ContactCastCollector::AddHit(const ShapeCastResult &inRes
 
 			mContacts.emplace_back();
 			Contact &contact = mContacts.back();
-			sFillContactProperties(contact, body, *this, inResult);
+			sFillContactProperties(contact, body, mUp, *this, inResult);
 			contact.mFraction = inResult.mFraction;
 
 			// Protection from excess of contact points
@@ -114,7 +119,7 @@ void CharacterVirtual::GetContactsAtPosition(Vec3Arg inPosition, Vec3Arg inMovem
 	outContacts.clear();
 
 	// Collide shape
-	ContactCollector collector(mSystem, mMaxNumHits, outContacts);
+	ContactCollector collector(mSystem, mMaxNumHits, mUp, outContacts);
 	CheckCollision(inPosition, mRotation, inMovementDirection, mPredictiveContactDistance, inShape, collector, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter);
 
 	// Reduce distance to contact by padding to ensure we stay away from the object by a little margin
@@ -139,7 +144,7 @@ void CharacterVirtual::RemoveConflictingContacts(TempContactList &ioContacts, Ig
 				Contact &contact2 = ioContacts[c2];
 				if (contact1.mBodyB == contact2.mBodyB // Only same body
 					&& contact2.mDistance <= -cMinRequiredPenetration // Only for penetrations
-					&& contact1.mNormal.Dot(contact2.mNormal) < 0.0f) // Only opposing normals
+					&& contact1.mContactNormal.Dot(contact2.mContactNormal) < 0.0f) // Only opposing normals
 				{
 					// Discard contacts with the least amount of penetration
 					if (contact1.mDistance < contact2.mDistance)
@@ -190,7 +195,7 @@ bool CharacterVirtual::GetFirstContactForSweep(Vec3Arg inPosition, Vec3Arg inDis
 	// Cast shape
 	TempContactList contacts(inAllocator);
 	contacts.reserve(mMaxNumHits);
-	ContactCastCollector collector(mSystem, inDisplacement, mMaxNumHits, inIgnoredContacts, contacts);
+	ContactCastCollector collector(mSystem, inDisplacement, mMaxNumHits, mUp, inIgnoredContacts, contacts);
 	ShapeCast shape_cast(mShape, Vec3::sReplicate(1.0f), start, inDisplacement);
 	mSystem->GetNarrowPhaseQuery().CastShape(shape_cast, settings, collector, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter);
 	if (contacts.empty())
@@ -202,7 +207,7 @@ bool CharacterVirtual::GetFirstContactForSweep(Vec3Arg inPosition, Vec3Arg inDis
 	// 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.mNormal.Dot(inDisplacement) < -mCollisionTolerance
+		if (c.mDistance + c.mContactNormal.Dot(inDisplacement) < -mCollisionTolerance
 			&& ValidateContact(c))
 		{
 			outContact = c;
@@ -218,7 +223,7 @@ bool CharacterVirtual::GetFirstContactForSweep(Vec3Arg inPosition, Vec3Arg inDis
 	// <=> d' = -p |d| / n dot d
 	// The new fraction of collision is then:
 	// f' = f - d' / |d| = f + p / n dot d
-	float dot = outContact.mNormal.Dot(inDisplacement);
+	float dot = outContact.mContactNormal.Dot(inDisplacement);
 	if (dot < 0.0f) // We should not divide by zero and we should only update the fraction if normal is pointing towards displacement
 		outContact.mFraction = max(0.0f, outContact.mFraction + mCharacterPadding / dot);
 	return true;
@@ -232,31 +237,31 @@ void CharacterVirtual::DetermineConstraints(TempContactList &inContacts, Constra
 
 		// Penetrating contact: Add a contact velocity that pushes the character out at the desired speed
 		if (c.mDistance < 0.0f)
-			contact_velocity -= c.mNormal * c.mDistance * mPenetrationRecoverySpeed;
+			contact_velocity -= c.mContactNormal * c.mDistance * mPenetrationRecoverySpeed;
 
 		// Convert to a constraint
 		outConstraints.emplace_back();
 		Constraint &constraint = outConstraints.back();
 		constraint.mContact = &c;
 		constraint.mLinearVelocity = contact_velocity;
-		constraint.mPlane = Plane(c.mNormal, c.mDistance);
+		constraint.mPlane = Plane(c.mContactNormal, c.mDistance);
 
 		// Next check if the angle is too steep and if it is add an additional constraint that holds the character back
-		if (IsSlopeTooSteep(c.mNormal))
+		if (IsSlopeTooSteep(c.mSurfaceNormal))
 		{
 			// Only take planes that point up
-			float dot = c.mNormal.Dot(mUp);
+			float dot = c.mSurfaceNormal.Dot(mUp);
 			if (dot > 0.0f)
 			{
 				// Make horizontal normal
-				Vec3 normal = (c.mNormal - dot * mUp).Normalized();
+				Vec3 normal = (c.mSurfaceNormal - dot * mUp).Normalized();
 
 				// Create a secondary constraint that blocks horizontal movement
 				outConstraints.emplace_back();
 				Constraint &vertical_constraint = outConstraints.back();
 				vertical_constraint.mContact = &c;
 				vertical_constraint.mLinearVelocity = contact_velocity.Dot(normal) * normal; // Project the contact velocity on the new normal so that both planes push at an equal rate
-				vertical_constraint.mPlane = Plane(normal, c.mDistance / normal.Dot(c.mNormal)); // Calculate the distance we have to travel horizontally to hit the contact plane
+				vertical_constraint.mPlane = Plane(normal, c.mDistance / normal.Dot(c.mSurfaceNormal)); // Calculate the distance we have to travel horizontally to hit the contact plane
 			}
 		}
 	}
@@ -273,7 +278,7 @@ bool CharacterVirtual::HandleContact(Vec3Arg inVelocity, Constraint &ioConstrain
 	// Send contact added event
 	CharacterContactSettings settings;
 	if (mListener != nullptr)
-		mListener->OnContactAdded(this, contact.mBodyB, contact.mSubShapeIDB, contact.mPosition, -contact.mNormal, settings);
+		mListener->OnContactAdded(this, contact.mBodyB, contact.mSubShapeIDB, contact.mPosition, -contact.mContactNormal, settings);
 	contact.mCanPushCharacter = settings.mCanPushCharacter;
 
 	// If body B cannot receive an impulse, we're done
@@ -290,7 +295,7 @@ bool CharacterVirtual::HandleContact(Vec3Arg inVelocity, Constraint &ioConstrain
 	constexpr float cDamping = 0.9f;
 	constexpr float cPenetrationResolution = 0.4f;
 	Vec3 relative_velocity = inVelocity - contact.mLinearVelocity;
-	float projected_velocity = relative_velocity.Dot(contact.mNormal);
+	float projected_velocity = relative_velocity.Dot(contact.mContactNormal);
 	float delta_velocity = -projected_velocity * cDamping - min(contact.mDistance, 0.0f) * cPenetrationResolution / inDeltaTime;
 
 	// Don't apply impulses if we're separating
@@ -304,7 +309,7 @@ bool CharacterVirtual::HandleContact(Vec3Arg inVelocity, Constraint &ioConstrain
 	float inverse_mass = motion_properties->GetInverseMass();
 
 	// Calculate the inverse of the mass of body B as seen at the contact point in the direction of the contact normal
-	Vec3 jacobian = (contact.mPosition - center_of_mass).Cross(contact.mNormal);
+	Vec3 jacobian = (contact.mPosition - center_of_mass).Cross(contact.mContactNormal);
 	float inv_effective_mass = inverse_inertia.Multiply3x3(jacobian).Dot(jacobian) + inverse_mass;
 
 	// Impulse P = M dv
@@ -315,10 +320,10 @@ bool CharacterVirtual::HandleContact(Vec3Arg inVelocity, Constraint &ioConstrain
 	impulse = min(impulse, max_impulse);
 
 	// Calculate the world space impulse to apply
-	Vec3 world_impulse = -impulse * contact.mNormal;
+	Vec3 world_impulse = -impulse * contact.mContactNormal;
 
 	// Add the impulse due to gravity working on the player: P = F dt = M g dt
-	float normal_dot_gravity = contact.mNormal.Dot(inGravity);
+	float normal_dot_gravity = contact.mContactNormal.Dot(inGravity);
 	if (normal_dot_gravity < 0.0f)
 		world_impulse -= (mMass * normal_dot_gravity / inGravity.Length() * inDeltaTime) * inGravity;
 
@@ -350,6 +355,9 @@ void CharacterVirtual::SolveConstraints(Vec3Arg inVelocity, Vec3Arg inGravity, f
 	// This is the velocity we use for the displacement, if we hit something it will be shortened
 	Vec3 velocity = inVelocity;
 
+	// Keep track of the last velocity that was applied to the character so that we can detect when the velocity reverses
+	Vec3 last_velocity = inVelocity;
+
 	// Start with no displacement
 	outDisplacement = Vec3::sZero();
 	outTimeSimulated = 0.0f;
@@ -524,7 +532,7 @@ void CharacterVirtual::SolveConstraints(Vec3Arg inVelocity, Vec3Arg inGravity, f
 
 		// Allow application to modify calculated velocity
 		if (mListener != nullptr)
-			mListener->OnContactSolve(this, constraint->mContact->mBodyB, constraint->mContact->mSubShapeIDB, constraint->mContact->mPosition, constraint->mContact->mNormal, constraint->mContact->mLinearVelocity, constraint->mContact->mMaterial, velocity, new_velocity);
+			mListener->OnContactSolve(this, constraint->mContact->mBodyB, constraint->mContact->mSubShapeIDB, constraint->mContact->mPosition, constraint->mContact->mContactNormal, constraint->mContact->mLinearVelocity, constraint->mContact->mMaterial, velocity, new_velocity);
 
 #ifdef JPH_DEBUG_RENDERER
 		if (inDrawConstraints)
@@ -557,6 +565,12 @@ void CharacterVirtual::SolveConstraints(Vec3Arg inVelocity, Vec3Arg inGravity, f
 		// If there's not enough velocity left, bail
 		if (velocity.LengthSq() < 1.0e-8f)
 			return;
+
+		// If the constraint has velocity we accept the new velocity, otherwise check that we didn't reverse velocity
+		if (!constraint->mLinearVelocity.IsNearZero(1.0e-8f))
+			last_velocity = constraint->mLinearVelocity;
+		else if (velocity.Dot(last_velocity) < 0.0f)
+			return;
 	}
 }
 
@@ -567,7 +581,7 @@ void CharacterVirtual::UpdateSupportingContact(bool inSkipContactVelocityCheck,
 	for (Contact &c : mActiveContacts)
 		if (!c.mWasDiscarded)
 			c.mHadCollision |= c.mDistance < mCollisionTolerance
-								&& (inSkipContactVelocityCheck || c.mNormal.Dot(mLinearVelocity - c.mLinearVelocity) <= 0.0f);
+								&& (inSkipContactVelocityCheck || c.mSurfaceNormal.Dot(mLinearVelocity - c.mLinearVelocity) <= 0.0f);
 
 	// Determine if we're supported or not
 	int num_supported = 0;
@@ -581,7 +595,7 @@ void CharacterVirtual::UpdateSupportingContact(bool inSkipContactVelocityCheck,
 		if (c.mHadCollision)
 		{
 			// Calculate the angle between the plane normal and the up direction
-			float cos_angle = c.mNormal.Dot(mUp);
+			float cos_angle = c.mSurfaceNormal.Dot(mUp);
 
 			// Find the contact with the normal that is pointing most upwards and store it in mSupportingContact
 			if (max_cos_angle < cos_angle)
@@ -600,7 +614,7 @@ void CharacterVirtual::UpdateSupportingContact(bool inSkipContactVelocityCheck,
 			// If the angle between the two is less than 85 degrees we also use it to calculate the average normal
 			if (cos_angle >= 0.08f)
 			{
-				avg_normal += c.mNormal;
+				avg_normal += c.mSurfaceNormal;
 				num_avg_normal++;
 
 				// For static or dynamic objects or for contacts that don't support us just take the contact velocity
@@ -703,11 +717,7 @@ void CharacterVirtual::UpdateSupportingContact(bool inSkipContactVelocityCheck,
 		float time_simulated;
 		IgnoredContactList ignored_contacts(inAllocator);
 		ignored_contacts.reserve(contacts.size());
-		SolveConstraints(-mUp, mSystem->GetGravity(), 1.0f, 1.0f, constraints, ignored_contacts, time_simulated, displacement, inAllocator
-		#ifdef JPH_DEBUG_RENDERER
-			, false
-		#endif // JPH_DEBUG_RENDERER
-			);
+		SolveConstraints(-mUp, mSystem->GetGravity(), 1.0f, 1.0f, constraints, ignored_contacts, time_simulated, displacement, inAllocator);
 
 		// If we're blocked then we're supported, otherwise we're sliding
 		float min_required_displacement_sq = Square(0.6f * mLastDeltaTime);
@@ -730,7 +740,11 @@ void CharacterVirtual::StoreActiveContacts(const TempContactList &inContacts, Te
 	UpdateSupportingContact(true, inAllocator);
 }
 
-void CharacterVirtual::MoveShape(Vec3 &ioPosition, Vec3Arg inVelocity, Vec3Arg inGravity, float inDeltaTime, ContactList *outActiveContacts, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, TempAllocator &inAllocator) const
+void CharacterVirtual::MoveShape(Vec3 &ioPosition, Vec3Arg inVelocity, Vec3Arg inGravity, float inDeltaTime, ContactList *outActiveContacts, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, TempAllocator &inAllocator
+#ifdef JPH_DEBUG_RENDERER
+	, bool inDrawConstraints
+#endif // JPH_DEBUG_RENDERER
+	) const
 {
 	Vec3 movement_direction = inVelocity.NormalizedOr(Vec3::sZero());
 
@@ -753,7 +767,8 @@ void CharacterVirtual::MoveShape(Vec3 &ioPosition, Vec3Arg inVelocity, Vec3Arg i
 		DetermineConstraints(contacts, constraints);
 
 #ifdef JPH_DEBUG_RENDERER
-		if (sDrawConstraints && iteration == 0)
+		bool draw_constraints = inDrawConstraints && iteration == 0;
+		if (draw_constraints)
 		{
 			for (const Constraint &c : constraints)
 			{
@@ -766,6 +781,7 @@ void CharacterVirtual::MoveShape(Vec3 &ioPosition, Vec3Arg inVelocity, Vec3Arg i
 
 				// Draw plane around the player position indicating the space that we can move
 				DebugRenderer::sInstance->DrawPlane(mPosition + dist_to_plane, c.mPlane.GetNormal(), Color::sCyan, 1.0f);
+				DebugRenderer::sInstance->DrawArrow(mPosition + dist_to_plane, mPosition + dist_to_plane + c.mContact->mSurfaceNormal, Color::sRed, 0.05f);
 			}
 		}
 #endif // JPH_DEBUG_RENDERER
@@ -775,7 +791,7 @@ void CharacterVirtual::MoveShape(Vec3 &ioPosition, Vec3Arg inVelocity, Vec3Arg i
 		float time_simulated;
 		SolveConstraints(inVelocity, inGravity, inDeltaTime, time_remaining, constraints, ignored_contacts, time_simulated, displacement, inAllocator
 		#ifdef JPH_DEBUG_RENDERER
-			, sDrawConstraints
+			, draw_constraints
 		#endif // JPH_DEBUG_RENDERER
 			);
 
@@ -811,7 +827,11 @@ void CharacterVirtual::Update(float inDeltaTime, Vec3Arg inGravity, const BroadP
 	mLastDeltaTime = inDeltaTime;
 
 	// Slide the shape through the world
-	MoveShape(mPosition, mLinearVelocity, inGravity, inDeltaTime, &mActiveContacts, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inAllocator);
+	MoveShape(mPosition, mLinearVelocity, inGravity, inDeltaTime, &mActiveContacts, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inAllocator
+	#ifdef JPH_DEBUG_RENDERER
+		, sDrawConstraints
+	#endif // JPH_DEBUG_RENDERER
+		);
 
 	// Determine the object that we're standing on
 	UpdateSupportingContact(false, inAllocator);
@@ -870,8 +890,8 @@ bool CharacterVirtual::CanWalkStairs(Vec3Arg inLinearVelocity) const
 	// 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
-			&& IsSlopeTooSteep(c.mNormal)) // Slope too steep
+			&& c.mSurfaceNormal.Dot(horizontal_velocity - c.mLinearVelocity) < 0.0f // Pushing into the contact
+			&& IsSlopeTooSteep(c.mSurfaceNormal)) // Slope too steep
 			return true;
 
 	return false;
@@ -896,7 +916,7 @@ bool CharacterVirtual::WalkStairs(float inDeltaTime, Vec3Arg inGravity, Vec3Arg
 #ifdef JPH_DEBUG_RENDERER
 	// Draw sweep up
 	if (sDrawWalkStairs)
-		DebugRenderer::sInstance->DrawArrow(mPosition, up_position, Color::sGrey, 0.01f);
+		DebugRenderer::sInstance->DrawArrow(mPosition, up_position, Color::sWhite, 0.01f);
 #endif // JPH_DEBUG_RENDERER
 
 	// Horizontal movement
@@ -908,7 +928,7 @@ bool CharacterVirtual::WalkStairs(float inDeltaTime, Vec3Arg inGravity, Vec3Arg
 #ifdef JPH_DEBUG_RENDERER
 	// Draw horizontal sweep
 	if (sDrawWalkStairs)
-		DebugRenderer::sInstance->DrawArrow(up_position, new_position, Color::sGrey, 0.01f);
+		DebugRenderer::sInstance->DrawArrow(up_position, new_position, Color::sWhite, 0.01f);
 #endif // JPH_DEBUG_RENDERER
 
 	// Move down towards the floor.
@@ -923,14 +943,14 @@ bool CharacterVirtual::WalkStairs(float inDeltaTime, Vec3Arg inGravity, Vec3Arg
 	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);
+		DebugRenderer::sInstance->DrawArrow(new_position, debug_pos, Color::sWhite, 0.01f);
+		DebugRenderer::sInstance->DrawArrow(contact.mPosition, contact.mPosition + contact.mSurfaceNormal, Color::sWhite, 0.01f);
+		mShape->Draw(DebugRenderer::sInstance, GetCenterOfMassTransform(debug_pos, mRotation, mShape), Vec3::sReplicate(1.0f), Color::sWhite, false, true);
 	}
 #endif // JPH_DEBUG_RENDERER
 
 	// Test for floor that will support the character
-	if (IsSlopeTooSteep(contact.mNormal))
+	if (IsSlopeTooSteep(contact.mSurfaceNormal))
 	{
 		// If no test position was provided, we cancel the stair walk
 		if (inStepForwardTest.IsNearZero())
@@ -940,23 +960,34 @@ bool CharacterVirtual::WalkStairs(float inDeltaTime, Vec3Arg inGravity, Vec3Arg
 		// 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;
+
+		// First sweep forward to the test position
 		Contact test_contact;
-		bool hit = GetFirstContactForSweep(test_position, down, test_contact, dummy_ignored_contacts, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inAllocator);
-		if (!hit)
-			return false;
+		if (!GetFirstContactForSweep(up_position, inStepForwardTest, test_contact, dummy_ignored_contacts, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inAllocator))
+		{
+			// Then sweep down
+			if (!GetFirstContactForSweep(test_position, down, test_contact, dummy_ignored_contacts, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inAllocator))
+				return false;
+		}
+		else
+		{
+			// If we didn't move down, set the 'down' fraction to zero
+			test_contact.mFraction = 0.0f;
+		}
 
 	#ifdef JPH_DEBUG_RENDERER
-		// Draw 2nd sweep down
+		// Draw 2nd sweep forward and down
 		if (sDrawWalkStairs)
 		{
 			Vec3 debug_pos = test_position + test_contact.mFraction * down; 
+			DebugRenderer::sInstance->DrawArrow(up_position, test_position, Color::sCyan, 0.01f);
 			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);
+			DebugRenderer::sInstance->DrawArrow(test_contact.mPosition, test_contact.mPosition + test_contact.mSurfaceNormal, 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 (IsSlopeTooSteep(test_contact.mNormal))
+		if (IsSlopeTooSteep(test_contact.mSurfaceNormal))
 			return false;
 	}
 

Jolt/Physics/Character/CharacterVirtual.h

@@ -209,7 +209,8 @@ private:
 	{
 		Vec3							mPosition;												///< Position where the character makes contact
 		Vec3							mLinearVelocity;										///< Velocity of the contact point
-		Vec3							mNormal;												///< Contact normal, pointing towards the character
+		Vec3							mContactNormal;											///< Contact normal, pointing towards the character
+		Vec3							mSurfaceNormal;											///< Surface normal of the contact
 		float							mDistance;												///< Distance to the contact <= 0 means that it is an actual contact, > 0 means predictive
 		float							mFraction;												///< Fraction along the path where this contact takes place
 		BodyID							mBodyB;													///< ID of body we're colliding with
@@ -253,10 +254,11 @@ private:
 	class ContactCollector : public CollideShapeCollector
 	{
 	public:
-										ContactCollector(PhysicsSystem *inSystem, uint inMaxHits, TempContactList &outContacts) : mSystem(inSystem), mContacts(outContacts), mMaxHits(inMaxHits) { }
+										ContactCollector(PhysicsSystem *inSystem, uint inMaxHits, Vec3Arg inUp, TempContactList &outContacts) : mUp(inUp), mSystem(inSystem), mContacts(outContacts), mMaxHits(inMaxHits) { }
 
 		virtual void					AddHit(const CollideShapeResult &inResult) override;
 
+		Vec3							mUp;
 		PhysicsSystem *					mSystem;
 		TempContactList &				mContacts;
 		uint							mMaxHits;
@@ -266,12 +268,13 @@ private:
 	class ContactCastCollector : public CastShapeCollector
 	{
 	public:
-										ContactCastCollector(PhysicsSystem *inSystem, Vec3Arg inDisplacement, uint inMaxHits, const IgnoredContactList &inIgnoredContacts, TempContactList &outContacts) : mSystem(inSystem), mDisplacement(inDisplacement), mIgnoredContacts(inIgnoredContacts), mContacts(outContacts), mMaxHits(inMaxHits) { }
+										ContactCastCollector(PhysicsSystem *inSystem, Vec3Arg inDisplacement, uint inMaxHits, Vec3Arg inUp, const IgnoredContactList &inIgnoredContacts, TempContactList &outContacts) : mDisplacement(inDisplacement), mUp(inUp), mSystem(inSystem), mIgnoredContacts(inIgnoredContacts), mContacts(outContacts), mMaxHits(inMaxHits) { }
 
 		virtual void					AddHit(const ShapeCastResult &inResult) override;
 
-		PhysicsSystem *					mSystem;
 		Vec3							mDisplacement;
+		Vec3							mUp;
+		PhysicsSystem *					mSystem;
 		const IgnoredContactList &		mIgnoredContacts;
 		TempContactList &				mContacts;
 		uint							mMaxHits;
@@ -279,10 +282,14 @@ private:
 
 	// Helper function to convert a Jolt collision result into a contact
 	template <class taCollector>
-	inline static void					sFillContactProperties(Contact &outContact, const Body &inBody, const taCollector &inCollector, const CollideShapeResult &inResult);
+	inline static void					sFillContactProperties(Contact &outContact, const Body &inBody, Vec3Arg inUp, const taCollector &inCollector, const CollideShapeResult &inResult);
 
 	// Move the shape from ioPosition and try to displace it by inVelocity * inDeltaTime, this will try to slide the shape along the world geometry
-	void								MoveShape(Vec3 &ioPosition, Vec3Arg inVelocity, Vec3Arg inGravity, float inDeltaTime, ContactList *outActiveContacts, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, TempAllocator &inAllocator) const;
+	void								MoveShape(Vec3 &ioPosition, Vec3Arg inVelocity, Vec3Arg inGravity, float inDeltaTime, ContactList *outActiveContacts, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, TempAllocator &inAllocator
+	#ifdef JPH_DEBUG_RENDERER
+		, bool inDrawConstraints = false
+	#endif // JPH_DEBUG_RENDERER
+		) const;
 
 	// Ask the callback if inContact is a valid contact point
 	bool								ValidateContact(const Contact &inContact) const;
@@ -299,7 +306,7 @@ private:
 	// Use the constraints to solve the displacement of the character. This will slide the character on the planes around the origin for as far as possible.
 	void								SolveConstraints(Vec3Arg inVelocity, Vec3Arg inGravity, float inDeltaTime, float inTimeRemaining, ConstraintList &ioConstraints, IgnoredContactList &ioIgnoredContacts, float &outTimeSimulated, Vec3 &outDisplacement, TempAllocator &inAllocator
 	#ifdef JPH_DEBUG_RENDERER
-		, bool inDrawConstraints
+		, bool inDrawConstraints = false
 	#endif // JPH_DEBUG_RENDERER
 		) const;
 

Samples/Tests/Character/CharacterBaseTest.cpp

@@ -286,7 +286,7 @@ void CharacterBaseTest::DrawCharacterState(const CharacterBase *inCharacter, Mat
 {
 	// Draw current location
 	// Drawing prior to update since the physics system state is also that prior to the simulation step (so that all detected collisions etc. make sense)
-	mDebugRenderer->DrawCoordinateSystem(inCharacterTransform);
+	mDebugRenderer->DrawCoordinateSystem(inCharacterTransform, 0.1f);
 
 	// Determine color
 	CharacterBase::EGroundState ground_state = inCharacter->GetGroundState();
@@ -313,7 +313,7 @@ void CharacterBaseTest::DrawCharacterState(const CharacterBase *inCharacter, Mat
 		Vec3 ground_velocity = inCharacter->GetGroundVelocity();
 
 		// Draw ground position
-		mDebugRenderer->DrawWireSphere(ground_position, 0.1f, Color::sRed);
+		mDebugRenderer->DrawMarker(ground_position, Color::sRed, 0.1f);
 		mDebugRenderer->DrawArrow(ground_position, ground_position + 2.0f * ground_normal, Color::sGreen, 0.1f);
 
 		// Draw ground velocity