Update Up Vector to Surface Normal (cont)

Hello!

EDIT: I found that this post ended up being a continuation of this other post I had that was never answered (the root cause was the same) and so I updated the name of this post to be pretty much the same; luckily this time, I have more information, so hopefully it can be answered:

Context

I have code running every frame that’s supposed to calculate the ideal up vector of a rectangular prism every frame, but it does not work as expected. Notice how in this video, the prism just trips over little objects when it should instead try to climb over them as if there was a ramp:

I need an algorithm to come up with the “ideal up vector” depending on the surface region

Take the following two-dimensional examples:

Example 1

Here, the ideal up vector is just <0,1> since the entire surface is level and the surface normal all throughout the surface is <0,1>

Example 2

Here, the ideal up vector is <0,1> because, although part of the surface is not level, the endpoints of the surface are level and have the same up vector of <0,1> and are higher than the middle of the surface, making it negligible.

Example 3

Here, the ideal up vector is rotated clockwise even though the surface normal all throughout is essentially the same (except for the sharp “cliff”). This is because the surface is not level and so in order to touch the majority of the surface, the ideal up vector has to be rotated clockwise. Think of how a book would lean on two small steps (which resemble the above surface). The book would not be horizontal; it would rotated clockwise and be touching both the top part and bottom part.

Example 4

Here, the ideal up vector is <0,1> even though none of the surface normals are completely vertical (the one on the bottom is a mistake; the normal there would be undefined). The reason it ends up being undefined is because all the normals are symmetrical. The average of them would end up being <0, 1> (completely vertical).

Example 5

Here, the ideal up vector is rotated clockwise. This is because the of the very high ray intersection point which almost makes the other surface normals almost negligible. It makes this situation similar to the third example where if you placed a book on this surface, it would essentially look the same as if you placed it on the two steps.

Summary

So in summary, I noticed the following relationships and continuities with each example:

  • The ideal up vector is not only dependent on the surface normals, but also the height of the intersection points associated with those normals in relation to each other. Take examples 3 and 5 where even though the normals were the same or variant, the height of one of the points really affected the situation, making the ideal up vector rotate to simulate reality where I made the comparison of a book leaning on those surfaces.
  • The ideal up vector appears to be some sort of average; maybe the average of a bunch of “processed normals,” where each processed normal is every given normal rotated somehow, depending on its associated height with respect to the height of the other points.
  • The ideal up vector seems to be highly influenced by the trend of the points—the path the points take if you were to graph them and connect them. This is why the height of a given point affects the rotation.

How can I expand these ideas to three dimensions?

Code

Helper function to rotate a vector a certain angle by an axis

local function rotateVectorByQuaternion(vector: Vector3, axis: Vector3, angle: number): Vector3
	local halfAngle = angle * 0.5
	local sinHalf = math.sin(halfAngle)
	
	local vecX = vector.X
	local vecY = vector.Y
	local vecZ = vector.Z
	
	local quatW = math.cos(halfAngle)
	local quatX = axis.X * sinHalf
	local quatY = axis.Y * sinHalf
	local quatZ = axis.Z * sinHalf
	
	return Vector3.new(
		vecX * (1 - 2 * (quatY * quatY + quatZ * quatZ)) + vecY * (2 * (quatX * quatY - quatZ * quatW)) + vecZ * (2 * (quatX * quatZ + quatY * quatW)),
		vecX * (2 * (quatX * quatY + quatZ * quatW)) + vecY * (1 - 2 * (quatX * quatX + quatZ * quatZ)) + vecZ * (2 * (quatY * quatZ - quatX * quatW)),
		vecX * (2 * (quatX * quatZ - quatY * quatW)) + vecY * (2 * (quatY * quatZ + quatX * quatW)) + vecZ * (1 - 2 * (quatX * quatX + quatY * quatY))
	)
end

Helper function to calculate a rotation matrix to rotate a certain angle around an axis

local function calculateRotationMatrix(axis: Vector3, angle: number): RotationMatrix
	local cosA = math.cos(angle)
	local sinA = math.sin(angle)
	local oneMinusCosA = 1 - cosA
	
	return {
		row1 = Vector3.new(
			cosA + axis.X * axis.X * oneMinusCosA,
			axis.X * axis.Y * oneMinusCosA - axis.Z * sinA,
			axis.X * axis.Z * oneMinusCosA + axis.Y * sinA
		),
		row2 = Vector3.new(
			axis.Y * axis.X * oneMinusCosA + axis.Z * sinA,
			cosA + axis.Y * axis.Y * oneMinusCosA,
			axis.Y * axis.Z * oneMinusCosA - axis.X * sinA
		),
		row3 = Vector3.new(
			axis.Z * axis.X * oneMinusCosA - axis.Y * sinA,
			axis.Z * axis.Y * oneMinusCosA + axis.X * sinA,
			cosA + axis.Z * axis.Z * oneMinusCosA
		)
	}
end

Function called every frame to update up vector to surface normal

local function updateGroundState()
	local centroid = scooterPhysicsBox.Position
	local currentUpVector = scooterPhysicsBox.CFrame.UpVector
	local currentRightVector = scooterPhysicsBox.CFrame.RightVector
	local currentLookVector = scooterPhysicsBox.CFrame.LookVector
	
	local numSamplePoints = 100
	local sampleRadius = maxDimension
	local raycastResults: {RaycastResult} = {}
	
	local lookRightCross = currentLookVector:Cross(currentRightVector)
	local rotationMatrix = calculateRotationMatrix(normalizedVector3(lookRightCross),
												   math.atan2(currentLookVector.Y, math.sqrt(currentLookVector.X * currentLookVector.X + currentLookVector.Z * currentLookVector.Z)))
	
	-- collect surface points and normals using raycasts
	for i = 0, numSamplePoints - 1, 1 do
		local angle = 2 * math.pi * i / numSamplePoints
		local samplePoint = Vector3.new(
			centroid.X + sampleRadius * math.cos(angle),
			centroid.Y + size.Y * 0.5,
			centroid.Z + sampleRadius * math.sin(angle)
		)
		
		samplePoint = Vector3.new(
			rotationMatrix.row1:Dot(samplePoint),
			rotationMatrix.row2:Dot(samplePoint),
			rotationMatrix.row3:Dot(samplePoint)
		)
		
		local result = workspace:Raycast(samplePoint, -currentUpVector * size.Y, raycastParams)
		if (result ~= nil) then
			table.insert(raycastResults, result)
		end
	end
	
	if next(raycastResults) == nil then
		surfaceNormal = Vector3.yAxis
		return
	end
	
	-- calculate processed normals and average them
	local sumProcessedNormal = Vector3.new(0, 0, 0)
	for _, result in ipairs(raycastResults) do
		local heightDifference = result.Position.Y - centroid.Y
		local processedNormal = rotateVectorByQuaternion(result.Normal, currentRightVector, heightDifference)
		
		sumProcessedNormal += processedNormal
	end
	
	local inverseNumResults = 1 / #raycastResults
	surfaceNormal = sumProcessedNormal * inverseNumResults
end
3 Likes

Bumping due to no responses and I haven’t figured it out yet

1 Like

Edited original post to include a video so what I say is easier to visualize.

1 Like

maybe try making the part not collide? if you need the part to interact with other object physics, maybe do a collision group, but i think it could be the best solution

or maybe add a raycast at the front, and when the part is moving foward at considerable speed, make it jump

1 Like

I have updated the original post to include my findings

1 Like

I am bumping bc I still haven’t found solution ;-; …

1 Like

I’ve made a character controller system similar to the one you have, and it detects the average surface normal like you do. But I don’t think the math is the problem on your controller. In the videos below, the debug shows a bunch of raycast point destinations which I use to get the surface normal. But my body is different and elevated off the ground.



I changed the collider body to a rectangle like you have, and it runs into the same issues. So if your math is right, I’d first start by elevating the frame a couple studs above the ground so the raycasts can actually hit something instead of the body colliding on the object infront of it and not getting any relevant normal information about the new elevation. I’d also ditch the square and go for a capsule body because it makes collisions feel better and spheres don’t usually get stuck on things with their smooth sides

I’d also change the position where you’re raycasting from. Right now in the image above, you’re casting all your rays from a center point, but that causes issues with the points that are casted at a steeper angle never hitting anything on flat surfaces if every ray is the same length.

In the image below, the orange lines are all of equal length. But if I were to cast them all with the origin being at the center point, I’d have to use Pythagoreans theorem to figure out what length (hypotenuse) the individual ray at the steeper angles (ones further away from the center) should be to actually hit any surface.
surface1

(mistake in this image, the top red arrow is supposed to be facing the same way as the surface and not straight up)
surface2

The way you’re doing it causes points to miss the target, even if the character length should be hitting the floor. So it compensates by adding force to the front and makes you lose balance.

Think of it like this, if you’re casting rays from a center point around the player it creates a “ball” effect where you “roll” as you move and that causes your wobble. So in the image below, you’d roll backwards

surface3

5 Likes

Thank you for your response @DataSigh! I appreciate the time and detail you put into your reply. I changed the code to raycast straight down from 100 different points around the centroid and that made the results of the normals and positions as expected! But it seems my math is quite wrong. What are you doing to get the up vector after getting an array of points and their associated normals? After sampling points and their associated surface normals, I end up with:

type SurfaceSample = {
  point: Vector3,
  normal: Vector3
}

...

local function updateGroundState()
  local samples: {SurfaceSample} = {}
  ...
  if next(samples) == nil then
    surfaceNormal = Vector3.yAxis
  else
    local sum = Vector3.zero
    for _, sample in ipairs(samples) do
      local heightDifference = sample.point.Y - centroid.Y
      local processedNormal = rotateVectorByQuaternion(sample.normal, rightVector, heightDifference)
      sum += processedNormal
    end

    surfaceNormal = sum / #samples
  end
end
2 Likes