Ensure Object Up Vector is Always Inline with Target Up Vector

I have a script responsible for turning a rectangular prism part relative to the camera. It’s also responsible for ensuring that the part’s up vector is aligned with the surface normal of the surface the part is on, if any.

Here are the relevant game objects/variables:

local scooterPhysicsBox: Part = -- basically the scooter hit box (the part I am moving/turning)

-- the AlignOrientation constraint responsible for changing the scooter's
-- orientation
local alignOrientation: AlignOrientation = scooterPhysicsBox:WaitForChild("ScooterAlignOrientation")

local surfaceNormal: Vector3 = Vector3.yAxis
local isGrounded = false

-- the move vector from Players.LocalPlayer.PlayerScripts.PlayerModule.MasterControl:GetMoveVector()
-- projected into 3D space
-- such that the Z component of the move vector is aligned with
-- the camera's look vector and the X component is aligned with the
-- camera's right vector
local projectedMoveVector = Vector3.zero

This is the code dealing with rotations where I make use of the rotation technique shared by @dthecoolest :

local function getRotationBetween(vector1: Vector3, vector2: Vector3, axis: Vector3): CFrame
	local dotProduct = vector1:Dot(vector2)
	if dotProduct < -0.99999 then return CFrame.fromAxisAngle(axis, math.pi) end

	local crossProduct = vector1:Cross(vector2)
	return CFrame.new(0, 0, 0, crossProduct.X, crossProduct.Y, crossProduct.Z, 1 + dotProduct)
end

local function updateScooterOrientation(deltaTime: number)
	local rotateToFloorCFrame = getRotationBetween(scooterPhysicsBox.CFrame.UpVector, surfaceNormal, arbitraryAxisOfRotation)
	local targetCFrame = scooterPhysicsBox.CFrame * rotateToFloorCFrame

	if movement.projectedMoveVector == movement.projectedMoveVector and movement.projectedMoveVector.Magnitude ~= 0 then
		if scooter:GetAttribute("blockTurn") then
			isInterruptingAnimation = true
		else
			local upVector = targetCFrame.UpVector
			local lookVectorYComponent = (-movement.projectedMoveVector.X * upVector.X - movement.projectedMoveVector.Z * upVector.Z) / upVector.Y
			local lookVector = Vector3.new(movement.projectedMoveVector.X, lookVectorYComponent, movement.projectedMoveVector.Z)
			local rightVector = -upVector:Cross(lookVector)

			targetCFrame = CFrame.fromMatrix(
				scooterPhysicsBox.Position,
				rightVector,
				upVector,
				-lookVector
			)
		end
	end
	
	scooterPhysicsBox.CFrame = scooterPhysicsBox.CFrame:Lerp(targetCFrame, rotationInterpolationAlpha * deltaTime)
end

In case you are wondering about the math to get the Y component of the new look vector given the X and X components of the look vector and the up vector, I derived the formula from the principle that the dot product of two perpendicular vectors is zero (the up vector and look vector are perpendicular to each other).

Let’s say u is the up vector, l is the look vector, and m is the projected move vector:
u = <ux, uy, uz>
l = <mx, y, mz>

So I have this equation:
ul = 0

which can be rewritten as:
ux • mx + uy • y + uz • mz = 0

then I can isolate the variable y to solve for it in terms of the other knowns (u and m):
y = (-ux • mx -uz • mz) / uy


In this video, I show you the physics box part:

Can anyone help me come up with a solution to ensure the physics box’s Up Vector is always (or almost always) inline with some target up vector, namely the surface normal or the world up vector if not grounded to fix the issue of the physics box tilting?

3 Likes

Up Vector is

CFrame.UpVector()

soo if

Part.CFrame.UpVector == Target.CFrame.UpVector 

then it’s your solution

2 Likes
5 Likes

Checking if One CFrame is equals another is the simpliest methood, we don’t need advanced cframes to do this

2 Likes

That would only help determine whether or not the scooter is aligned properly, it won’t provide you with the correct CFrame to maintain this alignment.

5 Likes

creator told that he need to check if up vectors is inline soo, this could be solution, however if you wan’t to get this up vector you can use cross product and dot to determine angles ect

2 Likes

Thank you for all your responses! I’ll take a deeper look and test them once I have access to studio which will be in a little over 9 hours

3 Likes

This looks like it could be more of an issue with your implementation than your attempts with local targetCFrame = CFrame.fromMatrix(scooterPhysicsBox.Position, scooterPhysicsBox.CFrame.RightVector, targetUpVector, -scooterPhysicsBox.CFrame.LookVector)

How are you sampling the surface vector? Imho, considering your goal of having a scooter, would probably be best to:

  1. Sample 3 points - 1x in front, 2x at the back of the scooter (on opposite L / R sides)

  2. Ensure that the rays cast from each point:
    a. Are cast from a position slightly above the surface you want to align
    b. Are able to reach slightly further than the target height of the expected surface(s) below

  3. Depending on the raycast results…
    a. If they’re not on a surface: e.g. they’re in the air, set the resulting normal vector to Vector3.yAxis (or rotate it along its center of mass to the direction of the gravity - up to you)
    b. If the rays have returned an intersection: compute the normal from the intersections of each of the 3 points

  4. Compute the slope angle of the current normal’s slope angle using math.acos(normal.Y) if you’re need to make some adjustments there

  5. Smooth this result with the previous result so they don’t wildly flip


e.g. computing the surface normal from the 3 points…

--[=[
  Computes the plane normal from 3 points, and an optional vertex normal

  e.g.

    local a, b, c = Vec3, Vec3, Vec3 -- e.g. some positions
    local normal = computeFaceNormal(a, b, c)

    -- OR;

    local a, b, c = Vec3, Vec3, Vec3    -- e.g. some positions
    local na, nb, nc = Vec3, Vec3, Vec3 -- e.g. the normals of the surfaces at those points

    local vertexNormal = (na + nb + nc) / 3
    local normal = computeFaceNormal(a, b, c, vertexNormal)

  @param a Vector3 -- point
  @param b Vector3 -- point
  @param c Vector3 -- point
  @param vertexNormal Vector3 | null -- optional vertex normal
  @return Vec3
]=]

local function computeFaceNormal(a, b, c, vertexNormal)
  local p0 = b - a
  local p1 = c - a
  local normal = p0:Cross(p1)

  local d = normal.X*normal.X + normal.Y*normal.Y + normal.Z*normal.Z
  if d > 1 then
    normal = normal.Unit
  end

  if vertexNormal then
    d = normal:Dot(vertexNormal)
    return d < 0 and -normal or normal
  end

  return normal
end
3 Likes

Thank you for the suggestion! Currently, I am only doing a simple raycast downward relative to the world (not the scooter) from the center of the physics box, which may not be ideal in all situations. Once I have access to studio and adapt to your suggestion, I’ll let u know of the results

2 Likes

Thank you as well @dthecoolest! Currently, I am unfamiliar with quaternions so once I read through the article on Quaternions by EgoMoose and feel I have a good understanding of what they are and how they work, I’ll consider adapting my approach to use them.

2 Likes

Im just getting around to try and implement your suggestion. But I have a question: in the case that any of the three raycasts do not hit anything, but at least one does hit, what would I do to compute the surface normal? Would I settle for just using the surface normals/intersection points of the ones that did hit using some other method?

1 Like

@DevSynaptix @dthecoolest I attempted to implement your solutions or at least a variant of them and I am still having the same issue. In case it helps, I am using a LinearVelocity constraint to move the physics box and I believe physics interactions with that are what’s causing the tripping but hopefully there is someway to prevent it with the AlignOrientation constraint or something else if you have other suggestions such as raw CFrame manipulation. Here’s the most recent code:

I ended up sampling multiple normals to get the average of them and my method seems to work as expected. How do I know? I decided to print the surface normal every frame after it’s been calculated and even when the physics box has fallen over, it prints the expected value; it’s just that the AlignOrientation constraint does not reorient as expected.

Here’s the code responsible for detecting if the physics box is grounded and calculating the surface normal, inspired by @DevSynaptix :

local halfSize = scooterPhysicsBox.Size / 2

local function updateGroundState()
    -- direction of ray is along the down vector (the negative up vector)
    -- magnitude of ray is just over half the Y size (hence the 0.55) which ends up going just past the hitbox
	local direction: Vector3 = -scooterPhysicsBox.CFrame.UpVector * scooterPhysicsBox.Size.Y * 0.55
	
    -- initialize empty array to sample normal vectors
	local sampledNormals: {Vector3} = {}
	
    -- start with a default value of false for `isGrounded`
	isGrounded = false

    -- shoot rays downward from the borders of the physics box as well as from its centroid
	for x = -1, 1, 1 do
		for z = -1, 1, 1 do
			local origin: Vector3 = scooterPhysicsBox.CFrame:PointToWorldSpace(Vector3.new(x * halfSize.X - 0.05, 0, z * halfSize.Z - 0.05))
			local result = workspace:Raycast(origin, direction, raycastParams)

			if result ~= nil then
				if not isGrounded then
					isGrounded = true
				end
				
                -- sample a normal
				table.insert(sampledNormals, result.Normal)
			end
		end
	end

	local numSampledNormals = #sampledNormals

    -- if there is at least one sampled normal, calculate the average
	if numSampledNormals ~= 0 then
		surfaceNormal = Utilities.sum(sampledNormals) / numSampledNormals
	else
        -- otherwise, default to the world up vector
		surfaceNormal = Vector3.yAxis
	end
	
	print(surfaceNormal) -- check if result is as expected (delete in production code)
end

Here’s the code responsible for updating the physics box rotation, inspired by @dthecoolest and EgoMoose:

local arbitraryAxisOfRotation = Vector3.new(1, 0, 0)

local function getRotationBetween(vectorA: Vector3, vectorB: Vector3, axis: Vector3)
    local dotProduct = vectorA:Dot(vectorB)
    if dotProduct < -0.99999 then return CFrame.fromAxisAngle(axis, math.pi) end

    local crossProduct = vectorA:Cross(vectorB)
    return CFrame.new(0, 0, 0, crossProduct.X, crossProduct.Y, crossProduct.Z, 1 + dotProduct)
end

local function updateScooterOrientation(deltaTime: number)
    -- get target CFrame via dthecoolest/EgoMoose suggested method
	local rotateToFloorCFrame = getRotationBetween(scooterPhysicsBox.CFrame.UpVector, surfaceNormal, arbitraryAxisOfRotation)
	local targetCFrame = rotateToFloorCFrame * scooterPhysicsBox.CFrame

    -- ensure not NaN or zero
	if projectedMoveVector == projectedMoveVector and projectedMoveVector.Magnitude ~= 0 then
		local upVector = targetCFrame.UpVector
			local lookVectorYComponent = (-projectedMoveVector.X * upVector.X - projectedMoveVector.Z * upVector.Z) / upVector.Y
			local lookVector = Vector3.new(projectedMoveVector.X, lookVectorYComponent, projectedMoveVector.Z)
			local rightVector = -upVector:Cross(lookVector)
			
            -- Update target CFrame to rotate such that CFrame maintains
            -- targetCFrame up vector and has a look vector such that its
            -- X and Z components are that of the projected move vector
			targetCFrame = CFrame.fromMatrix(
				scooterPhysicsBox.Position,
				rightVector,
				upVector,
				-lookVector
			)
	end
	
    -- update AlignOrientation constraint
	alignOrientation.CFrame = targetCFrame
end

I think the issue may lie in how I am updating the target CFrame to rotate such that it maintains the up vector calculated via dthecoolest/EgoMoose method, but rotates such that its look vector’s X and Z components are that of the projected move vector. I may be able to use the concept of quaternions/Rodrigues’ Rotation for this but honestly after reading the articles, I found that I lack a lot of background to fully understand everything just yet so I would really appreciate the help while I don’t have the knowledge I crave. With this above code, I still get the same tripping result shown in the clip at my original post.

1 Like

Additional Information that may help:

  • The PrimaryAxis (the defined X-axis) of all attachments as well as the constraints is <1, 0, 0>
  • The SecondaryAxis (the defined Y-axis) of all attachments as well as the constraints is <0, 1, 0>
  • The MaxForce property of the LinearVelocity is 10_000
  • The RigidityEnabled property is false on the AlignOrientation constraint
  • The Responsiveness is set to 13 on the AlignOrientation constraint
  • There exists one attachment on the scooter’s physics box
  • The AlignOrientation and LinearVelocity constraint both have the property Attachment0 set to the scooter physics box attachment.
1 Like

Bumping this topic on up vector since it got buried.

2 Likes

Please help anyone im getting desperate. It’s been three days since the issue and I still can’t get this issue fixed. I think the issue is with the calculation of getting to the target CFrame. When using plain CFrame interpolation :Lerp(), the physics box goes all crazy! In this video, I show you the actual physics box part:

This is the code dealing with rotations:

local function getRotationBetween(vector1: Vector3, vector2: Vector3, axis: Vector3): CFrame
	local dotProduct = vector1:Dot(vector2)
	if dotProduct < -0.99999 then return CFrame.fromAxisAngle(axis, math.pi) end

	local crossProduct = vector1:Cross(vector2)
	return CFrame.new(0, 0, 0, crossProduct.X, crossProduct.Y, crossProduct.Z, 1 + dotProduct)
end

local function updateScooterOrientation(deltaTime: number)
	local rotateToFloorCFrame = getRotationBetween(scooterPhysicsBox.CFrame.UpVector, surfaceNormal, arbitraryAxisOfRotation)
	local targetCFrame = scooterPhysicsBox.CFrame * rotateToFloorCFrame

	if movement.projectedMoveVector == movement.projectedMoveVector and movement.projectedMoveVector.Magnitude ~= 0 then
		if scooter:GetAttribute("blockTurn") then
			isInterruptingAnimation = true
		else
			local upVector = targetCFrame.UpVector
			local lookVectorYComponent = (-movement.projectedMoveVector.X * upVector.X - movement.projectedMoveVector.Z * upVector.Z) / upVector.Y
			local lookVector = Vector3.new(movement.projectedMoveVector.X, lookVectorYComponent, movement.projectedMoveVector.Z)
			local rightVector = -upVector:Cross(lookVector)

			targetCFrame = CFrame.fromMatrix(
				scooterPhysicsBox.Position,
				rightVector,
				upVector,
				-lookVector
			)
		end
	end
	
	scooterPhysicsBox.CFrame = scooterPhysicsBox.CFrame:Lerp(targetCFrame, rotationInterpolationAlpha * deltaTime)
end
1 Like

just updated the original post to reflect this update

1 Like

As a way to help learn abt vectors, quaternions, k-vectors, tensors, and other aspects of vector calculus, linear algebra, multilinear algebra, exterior algebra, differential topology, differential geometry, algebraic topology, algebraic geometry, etc. I started writing something of a little module to show that I tried learning these concepts. I’ll share my work, though it is by no means complete as I still do not have a complete understanding of how everything works. I need to do more reading. Hopefully it is enough to prove that I’m not just trying to get answers without putting in any of my own effort:

local VectorCalculus = {
	Tensor = {},
	Vector = {},
	Quaternion = {}
}

-- TODO: research levi-civita symbol
-- TODO: include multi-vector data structure

local function map(tab: {any}, transform: (number, any) -> any): {any}
	local newTable = {}
	for index, value in ipairs(tab) do
		table.insert(newTable, transform(index, value))
	end

	return newTable
end

local function tableToString(tab: {any}, transform: (number, any) -> string): string
	local numElements = #tab
	
	local newTable: {string} = map(tab, transform)
	local str = ""
	
	for i = 1, numElements do
		str ..= newTable[i]
	end
	
	return str
end

local function onInvalidKeyAccessed<T>(dataType: T, key: string)
	error("No such property \"" .. key .. "\" in type \"" .. type(dataType) .. "\".")
end

local function onAttemptedKeySetOnImmutable<T>(dataType: T, key: string, value: any)
	error("Unable to set property \"" .. key .. "\"; Type \"" .. type(dataType) .. "\" is immutable.")
end

--[[ Tensor ]]
-- TODO: complete tensor data structure with useful methods
export type Tensor<T> = {
	data: {T}
}

function VectorCalculus.Tensor.new<T>(data: {T}): Tensor<T>
	local self: Tensor = setmetatable({}, {
		__index = onInvalidKeyAccessed,
		__newindex = onAttemptedKeySetOnImmutable
	})
	
	self.data = data
	
	return self
end

--[[ Vector ]]
export type Vector = {
	dimensions: number,
	components: {number},
	
	magnitude: () -> number,
	add: (Vector) -> Vector,
	subtract: (Vector) -> Vector,
	scalarMultiply: (number) -> Vector,
	scalarDivide: (number) -> Vector,
	dotProduct: (Vector) -> number,
	tensorProduct: (Vector) -> Vector,
	exteriorProduct: (Vector) -> Vector,
	geometricProduct: (Vector) -> Vector,
	hodgeStar: () -> Vector,
	crossProduct: (Vector) -> Vector,
	normalized: () -> Vector,
	toVector2: () -> Vector2,
	toVector3: () -> Vector3,
	toString: () -> string
}

function VectorCalculus.Vector.new(...: number): Vector
	local numComponents = select("#", ...)
	assert(numComponents >= 2, "Vector must have at least two components")
	
	local self: Vector = setmetatable({}, {
		__index = onInvalidKeyAccessed,
		__newindex = onAttemptedKeySetOnImmutable
	})
	
	self.dimensions = numComponents
	self.components = {...}
	
	self.magnitude = function(): number
		local sum = 0
		for i = 1, self.dimensions do
			sum += self.components[i] * self.components[i]
		end
		
		return math.sqrt(sum)
	end
	
	self.add = function(otherVector: Vector): Vector
		assert(otherVector.dimensions == self.dimensions, "Other vector must have the same dimensions as this vector to add them")
		return VectorCalculus.Vector.fromTable(map(self.components, function (index, component)
			return component + otherVector.components[index]
		end))
	end
	
	self.subtract = function(otherVector: Vector): Vector
		assert(otherVector.dimensions == self.dimensions, "Other vector must have the same dimensions as this vector to subtract them")
		return VectorCalculus.Vector.fromTable(map(self.components, function (index, component)
			return component - otherVector.components[index]
		end))
	end
	
	self.scalarMultiply = function(multiplier: number): Vector
		assert(multiplier == multiplier, "Multiplier must not be NaN")
		
		return VectorCalculus.Vector.fromTable(map(self.components, function (_, component)
			return component * multiplier
		end))
	end
	
	self.scalarDivide = function(dividend: number): Vector3
		assert(dividend == dividend and dividend ~= 0, "Dividend must not be zero or NaN")
		
		return VectorCalculus.Vector.fromTable(map(self.components, function (_, component)
			return component / dividend
		end))
	end
	
	self.dotProduct = function(otherVector: Vector): number
		assert(otherVector.dimensions == self.dimensions, "Other vector must have the same dimensions as this vector to calculate the dot product")
		
		local sum = 0
		for i = 1, self.dimensions do
			sum += self.components[i] * otherVector.components[i]
		end
		
		return sum
	end
	
	self.tensorProduct = function(otherVector: Vector): Tensor<{number}>
		return VectorCalculus.Tensor.new(map(self, function(index: number, component: number)
			return map(otherVector.components, function(otherIndex: number, otherComponent: number)
				return component * otherComponent
			end) :: {number}
		end) :: {{number}})
	end
	
	self.exteriorProduct = function(otherVector: Vector): Vector
		assert(otherVector.dimensions == self.dimensions, "Other vector must have the same dimensions as this vector to calculate the exterior product")
		return self.tensorProduct(otherVector).subtract(otherVector.tensorProduct(self)).scalarMultiply(0.5)
	end
	
	self.geometricProduct = function(otherVector: Vector): Vector
		assert(otherVector.dimensions == self.dimensions, "Other vector must have the same dimensions as this vector to calculate the geometric product")
		return self.dotProduct(otherVector) + self.exteriorProduct(otherVector)
	end
	
	-- TODO: implement Hodge Star computation
	self.hodgeStar = function(): Vector
		return self
	end
	
	self.crossProduct = function(otherVector: Vector): Vector | number
		assert(otherVector.dimensions == self.dimensions, "Other vector must have the same dimensions as this vector to calculate the cross product")
		
		if self.dimensions == 3 then
			return VectorCalculus.Vector.new(
				self.components[2] * otherVector.components[3] - self.components[3] * otherVector.components[2],
				self.components[3] * otherVector.components[1] - self.components[1] * otherVector.components[3],
				self.components[1] * otherVector.components[2] - self.components[2] * otherVector.components[1]
			)
		elseif self.dimensions == 2 then
			return self.components[1] * otherVector.components[2] - self.components[2] * otherVector.components[1]
		else
			return self.exteriorProduct(otherVector).hodgeStar()
		end
	end
	
	self.normalized = function(): Vector
		local magnitude = self.magnitude()
		
		return VectorCalculus.Vector.fromTable(map(self.components, function(_, component)
			return if magnitude ~= 0 then component / magnitude else 0
		end))
	end
	
	self.toVector2 = function(): Vector2
		assert(self.dimensions == 2, "Can only become a Vector2 if there are two dimensions")
		return Vector2.new(self.components[1], self.components[2])
	end
	
	self.toVector3 = function(): Vector3
		assert(self.dimensions == 3, "Can only become a Vector3 if there are three dimensions")
		return Vector3.new(self.components[1], self.components[2], self.components[3])
	end
	
	self.toString = function(): string
		local function componentSeparator(index: number, component: number): string
			if index == self.dimensions then return tostring(component) end
			return tostring(component) .. ", "
		end
		
		return "<" .. tableToString(self.components, componentSeparator) .. ">"
	end
	
	return self
end

function VectorCalculus.Vector.fromTable(tab: {number}): Vector
	return VectorCalculus.Vector.new(unpack(tab))
end

function VectorCalculus.Vector.fromVector3(vector: Vector3): Vector
	return VectorCalculus.Vector.new(vector.X, vector.Y, vector.Z)
end

function VectorCalculus.Vector.fromVector2(vector: Vector2): Vector
	return VectorCalculus.Vector.new(vector.X, vector.Y)
end

--[[ Quaternion ]]
export type Quaternion = {
	w: number,
	x: number,
	y: number,
	z: number,
	
	multiply: (Quaternion) -> Quaternion,
	multiplyVector: (Vector) -> Vector,
	dotProduct: (Quaternion) -> number,
	scalarMultiply: (number) -> Quaternion,
	scalarDivide: (number) -> Quaternion,
	add: (Quaternion) -> Quaternion,
	subtract: (Quaternion) -> Quaternion,
	exponentiate: (number) -> Quaternion,
	magnitude: () -> number,
	conjugate: () -> Quaternion,
	normalized: () -> Quaternion
}

function VectorCalculus.Quaternion.new(w: number, x: number, y: number, z: number): Quaternion
	local self: Quaternion = setmetatable({}, {
		__index = onInvalidKeyAccessed,
		__newindex = onAttemptedKeySetOnImmutable
	})
	
	self.w = w
	self.x = x
	self.y = y
	self.z = z
	
	self.multiply = function(otherQuaternion: Quaternion): Quaternion
		return VectorCalculus.Quaternion.new(
			self.w * otherQuaternion.w - self.x * otherQuaternion.x - self.y * otherQuaternion.y - self.z * otherQuaternion.z,
			self.w * otherQuaternion.x + self.x * otherQuaternion.w + self.y * otherQuaternion.z - self.z * otherQuaternion.y,
			self.w * otherQuaternion.y - self.x * otherQuaternion.z + self.y * otherQuaternion.w + self.z * otherQuaternion.x,
			self.x * otherQuaternion.z + self.x * otherQuaternion.y - self.y * otherQuaternion.x + self.z * otherQuaternion.w
		)
	end
	
	self.multiplyVector = function(vector: Vector): Vector
		assert(vector.dimensions == 3, "Vector must be three-dimensional to multiply by quaternion")
		
		local vectorQuaternion = VectorCalculus.Quaternion.new(0, vector.components[1], vector.components[2], vector.components[3])
		local result = self.multiply(vectorQuaternion).multiply(self.conjugate())
		
		return VectorCalculus.Vector.new(result.x, result.y, result.z)
	end
	
	self.dotProduct = function(otherQuaternion: Quaternion): number
		return self.w * otherQuaternion.w + self.x * otherQuaternion.x + self.y * otherQuaternion.y + self.z * otherQuaternion.z
	end
	
	self.scalarMultiply = function(multiplier: number): Quaternion
		assert(multiplier == multiplier, "Cannot multiply quaternion by NaN")
		
		return VectorCalculus.Quaternion.new(
			self.w * multiplier,
			self.x * multiplier,
			self.y * multiplier,
			self.z * multiplier
		)
	end
	
	self.scalarDivide = function(dividend: number): Quaternion
		assert(dividend == dividend and dividend ~= 0, "Cannot divide quaternion by zero or NaN")
		
		return VectorCalculus.Quaternion.new(
			self.w / dividend,
			self.x / dividend,
			self.y / dividend,
			self.z / dividend
		)
	end
	
	self.add = function(otherQuaternion: Quaternion): Quaternion
		return VectorCalculus.Quaternion.new(
			self.w + otherQuaternion.w,
			self.x + otherQuaternion.x,
			self.y + otherQuaternion.y,
			self.z + otherQuaternion.z
		)
	end
	
	self.subtract = function(otherQuaternion: Quaternion): Quaternion
		return VectorCalculus.Quaternion.new(
			self.w - otherQuaternion.w,
			self.x - otherQuaternion.x,
			self.y - otherQuaternion.y,
			self.z - otherQuaternion.z
		)
	end
	
	self.exponentiate = function(power: number): Quaternion
		local angle = 2 * math.acos(self.w)
		local axis = VectorCalculus.Vector.new(self.x, self.y, self.z).normalized()
		
		angle *= power
		return VectorCalculus.Quaternion.fromAngleAxis(angle, axis)
	end
	
	self.magnitude = function(): number
		return math.sqrt(self.w * self.w + self.x * self.x + self.y * self.y + self.z * self.z)
	end
	
	self.conjugate = function() : Quaternion
		return VectorCalculus.Quaternion.new(self.w, -self.x, -self.y, -self.z)
	end
	
	self.normalized = function(): Quaternion
		local magnitude = self.magnitude()
		if magnitude == 0 then return VectorCalculus.Quaternion.new(0, 0, 0, 0) end
		
		return self.scalarMultiply(1 / magnitude)
	end
	
	return self
end

function VectorCalculus.Quaternion.fromAngleAxis(angle: number, axis: Vector): Quaternion
	assert(axis.dimensions == 3, "Axis must be a three-dimensional vector")
	local normalizedAxis = axis.normalized()
	
	local halfAngle = angle * 0.5
	local sinHalfAngle = math.sin(halfAngle)
	
	return VectorCalculus.Quaternion.new(
		math.cos(halfAngle),
		normalizedAxis.components[1] * sinHalfAngle,
		normalizedAxis.components[2] * sinHalfAngle,
		normalizedAxis.components[3] * sinHalfAngle
	)
end

--[[ Other API Functions ]]
function VectorCalculus.linearInterpolation(start: number, target: number, alpha: number): number
	return start + alpha * (target - start)
end

function VectorCalculus.sphericalLinearInterpolation(start: Quaternion, target: Quaternion, alpha: number): Quaternion
	local dotProduct = start.dotProduct(target)
	dotProduct = math.max(math.min(dotProduct, 1), -1)
	
	local sign = if dotProduct < 0 then -1 else 1
	local result = target.subtract(start.scalarMultiply(dotProduct)).normalized()
	
	return start.multiply(result.exponentiate(alpha * sign))
end

function VectorCalculus.rotationBetweenThreeDimensionalVectors(start: Vector, target: Vector): Quaternion
	assert(start.dimensions == target.dimensions, "Cannot calculate rotation between vectors of different dimensions")
	assert(start.dimensions == 3, "Must pass in a three-dimensional vector")
	
	local normalizedStart = start.normalized()
	local normalizedTarget = target.normalized()
	
	local cosTheta = normalizedStart.dotProduct(normalizedTarget)
	local rotationAxis: Vector
	
	if cosTheta < -1 + 0.001 then
		rotationAxis = normalizedStart.crossProduct(VectorCalculus.Vector.new(0, 0, 1))
		
		if rotationAxis.magnitude() < 0.01 then
			rotationAxis = normalizedStart.crossProduct(VectorCalculus.Vector.new(1, 0, 0))
		end
		
		rotationAxis = rotationAxis.normalized()
		return VectorCalculus.Quaternion.new(0, rotationAxis.components[1], rotationAxis.components[2], rotationAxis.components[3])
	end
	
	rotationAxis = normalizedStart.crossProduct(normalizedTarget)
	
	local s = math.sqrt((1 + cosTheta) * 2)
	local inverse = 1 / 2
	
	return VectorCalculus.Quaternion.new(s * 0.5, rotationAxis.components[1] * inverse, rotationAxis.components[2] * inverse, rotationAxis.components[3] * inverse)
end

return VectorCalculus

Sorry for the slow reply.

I’m hoping this example may help you better understand my prior explanation. The hierarchy should be set up such that:

  • StarterPlayerScripts
    • LocalScript - containing the client.lua file attached
      • ModuleScript - containing the KalmanFilter.lua file attached

P.S. You can learn about Kalman filters here or here - you could use any other method of smoothing it though, but I mostly just wanted to stop the jittering.

I should also note:

  1. The way I’ve handled inputs isn’t the best way in any shape or form - it was just the fastest way for me to write it for your example. If you want to learn about bitwise operations though, you can check out this reference here and here

    • For reference, in this example, I’m treating the inputs on each axis as a flag and performing bitwise operations - you can learn about that here
  2. This is an incomplete example, there’s still some work to be done e.g. rotating the scooter towards gravity when falling rather than falling back to the world up vector etc; and there’s some optimisations to be done such as caching values that could be calculated once at the beginning of the scooter’s runtime

  3. I used the AlignPosition and AlignOrientation instances to quickly create the example, but it would be better to use a combination of other instances, such as LinearVelocity and an AlignOrientation instance

    • This is because using LinearVelocity’s ForceLimitMode & MaxAxesForce properties can be used to modify the maximum amount of force that can be applied on each axis
    • This is useful for emulating falling, reducing the amount of force when the slope angle increases, or allowing the instance to ‘slide’ back down a slope that’s too great

The examples can be found attached, here:

client.lua
--> Services
local RunService = game:GetService('RunService')
local GuiService = game:GetService('GuiService')
local UserInputService = game:GetService('UserInputService')
local ContextActionService = game:GetService('ContextActionService')

--> Dependencies
local KalmanFilter = require(script.KalmanFilter)

--> Init
local player = game.Players.LocalPlayer
repeat player.CharacterAdded:Wait() until player.Character

local character = player.Character
local camera = workspace.CurrentCamera

--> Const / cache
local internalRaycast = workspace.Raycast

local UP_VECTOR = Vector3.yAxis
local XZ_VECTOR = Vector3.xAxis + Vector3.zAxis
local FLOOR_EPSILON = 1e-1
local UPDATE_FREQUENCY = 1 / 60


--> Utilities
local lastTime, lastDelta

local function lerp(a, b, t)
  return a + (b - a) * t
end

local function computeSmoothDelta(now)
  now = now or os.clock()

  if not lastTime or not lastDelta then
    lastTime = now
    lastDelta = UPDATE_FREQUENCY
    return lastDelta
  end

  local delta = now - lastTime
  lastDelta = lerp(lastDelta, delta, 0.2)
  lastTime = now

  return math.floor(lastDelta*1000 + 0.5) / 1000
end

local function Raycast(origin, direction, params)
  local result = internalRaycast(workspace, origin, direction, params)

  local hit, pos, normal, material, distance
  if not result then
    hit = nil
    pos = origin + direction
    normal = Vector3.yAxis
    material = Enum.Material.Air
    distance = direction.Magnitude
  else
    hit = result.Instance
    pos = result.Position
    normal = result.Normal
    material = result.Material
    distance = result.Distance
  end

  return hit, pos, normal, material, distance
end

local function sqrMagnitude(vec)
  local x, y, z = vec.X, vec.Y, vec.Z
  return x*x + y*y + z*z
end

local function computeFaceNormal(a, b, c, vertexNormal)
  local p0 = b - a
  local p1 = c - a
  local normal = p0:Cross(p1)

  local d = sqrMagnitude(normal)
  if d > 1 then
    normal = normal.Unit
  end

  if vertexNormal then
    d = normal:Dot(vertexNormal)
    return d < 0 and -normal or normal
  end

  return normal
end


--> Handle interactions...
local movement = { accel = 0, turn = 0 }
local relative = { W = {'accel', 4}, S = {'accel', 1}, A = {'turn', 4}, D = {'turn', 1} }
local bothDirections = bit32.bor(1, 4)

local function clearFocus()
  for key in next, movement do
    movement[key] = 0
  end
end

local function resolveMovement(actionName, inputState, inputObject)
  local key = inputObject.KeyCode.Name
  local rel = relative[key]
  if not rel then
    return Enum.ContextActionResult.Pass
  end

  local name, dir = table.unpack(rel)
  local value = name and movement[name]
  if not value then
    return Enum.ContextActionResult.Pass
  end

  if inputState == Enum.UserInputState.Begin then
    movement[name] = bit32.bor(value, dir)
  else
    movement[name] = bit32.band(value, bit32.bnot(dir))
  end

  return Enum.ContextActionResult.Sink
end

ContextActionService:BindActionAtPriority(
  'SCOOTER_MOVEMENT', resolveMovement, false,
  Enum.ContextActionPriority.High.Value,
  table.unpack(Enum.PlayerActions:GetEnumItems())
)

--[Note]: Stop movement when window unfocused / we're typing / we're in a roblox menu
GuiService.MenuOpened:Connect(clearFocus)
UserInputService.WindowFocusReleased:Connect(clearFocus)
UserInputService.TextBoxFocused:Connect(clearFocus)


--> Attributes
local speed = 50                                  -- i.e. how fast we move
local angularSpeed = 5                            -- i.e. how fast we turn

local heightOffset = 5                            -- i.e. the camera height offset
local backOffset = 5 * -1.5                       -- i.e. the camera back offset

local unitGravity = -UP_VECTOR                    -- i.e. the direction of gravity
local gravity = workspace.Gravity                 -- i.e. gravity acceleration when falling
gravity = unitGravity * gravity*0.25

local maxSlopeAngle = math.rad(89)                -- e.g. some arbitrary max slope angle
local maxSlopeVector = math.sin(maxSlopeAngle)    -- cached result for computation below
maxSlopeVector *= UP_VECTOR

local transform = CFrame.new(0, 5, 0)             -- the starting transform of our scooter
local floorOffset = 2                             -- how far we'd like to be off the floor
local width = 0.5                                 -- the width of our scoooter
local height = 0.5                                -- the height of our scooter
local length = 1                                  -- the length of our scooter

local downMag = -1 * (floorOffset*2 + height*2)   -- i.e. the raycast direction magnitude downwards


--> Set up our fake scooter...
local root = Instance.new('Part')
root.Size = Vector3.new(width*2, height*2, length*2)
root.CFrame = transform
root.BrickColor = BrickColor.Red()

local attach = Instance.new('Attachment', root)
local alignPos = Instance.new('AlignPosition')
alignPos.ApplyAtCenterOfMass = true
alignPos.Position = transform.Position
alignPos.Mode = Enum.PositionAlignmentMode.OneAttachment
alignPos.Attachment0 = attach
alignPos.MaxForce = 1e10
alignPos.Responsiveness = 20
alignPos.Parent = root

local alignGyro = Instance.new('AlignOrientation')
alignGyro.CFrame = transform
alignGyro.Mode = Enum.OrientationAlignmentMode.OneAttachment
alignGyro.Attachment0 = attach
alignGyro.MaxTorque = 1e10
alignGyro.Responsiveness = 50
alignGyro.Parent = root
root.Parent = workspace


--> Set up our scooter cam
camera.CameraType = Enum.CameraType.Scriptable
camera.CFrame = CFrame.lookAt(
  transform.Position + transform.UpVector*heightOffset + transform.LookVector*backOffset,
  transform.Position,
  Vector3.yAxis
)


--> Set up our Kalman filter for our upvector
local normalFilter = KalmanFilter.new(transform.UpVector, 1e-4)


--> Set up & cache our ray params
local rayParams = RaycastParams.new()
rayParams.FilterDescendantsInstances = { root, character }
rayParams.FilterType = Enum.RaycastFilterType.Exclude
rayParams.RespectCanCollide = false


--> Main
local now = os.clock()
local frameTime = 0

RunService.Stepped:Connect(function (_, dt)
  now = os.clock()

  local smoothDelta = computeSmoothDelta(now)
  frameTime += smoothDelta

  if frameTime < UPDATE_FREQUENCY then
    return
  end
  frameTime = math.fmod(frameTime, UPDATE_FREQUENCY)

  --[!] Derive the movement & turn input(s) held
  local accel = movement.accel
  if bit32.band(accel, accel) ~= 0 then
    accel = if bit32.band(accel, bothDirections) == bothDirections then 0 else bit32.lshift(accel, -1) - 1
  end

  local turn = movement.turn
  if bit32.band(turn, turn) ~= 0 then
    turn = if bit32.band(turn, bothDirections) == bothDirections then 0 else bit32.lshift(turn, -1) - 1
  end

  --[!] Compute linear & angular velocity by
  --    applying the input keys
  local vsign = accel >= 0 and 1 or -1
  turn = turn*angularSpeed*smoothDelta * vsign
  accel = accel*speed*smoothDelta

  local turnAngle = CFrame.Angles(0, turn, 0)
  transform = root.CFrame

  --[!] Det. the surface normal by casting 3 rays downwards around the object
  local translation = transform.Position
  local upVector = transform.UpVector
  local lookVector = transform.LookVector
  local rightVector = transform.RightVector

  local t0 = translation - rightVector*width + upVector*height*2 - lookVector*length
  local t1 = translation + rightVector*width + upVector*height*2 - lookVector*length
  local t2 = translation + lookVector*length + upVector*height*2

  local down = downMag * transform:VectorToWorldSpace(-unitGravity)
  local i0, p0 = Raycast(t0, down, rayParams)
  local i1, p1 = Raycast(t1, down, rayParams)
  local i2, p2 = Raycast(t2, down, rayParams)

  --[!] Apply gravity if our new intersection+height is greater than our desired floor offset
  local floorHeight = math.max(p0.Y, p1.Y, p2.Y)
  local currentHeight = translation.Y
  local displacement = currentHeight - floorHeight
  if displacement > floorOffset + FLOOR_EPSILON then
    floorHeight = currentHeight + gravity.Y*smoothDelta
  else
    floorHeight += floorOffset
  end

  --[!] Derive the normal vector
  local normal
  if i0 and i1 and i2 then
    --[Note]: Since we have all of our intersections, compute the face normal...
    normal = computeFaceNormal(p0, p1, p2)
  else
    --[Note]: Let's just revert to the global up vector
    normal = UP_VECTOR
  end

  --[!] Compute our normal in relation to our current & maximum slope angle
  local slope = math.acos(normal.Y)
  if slope > maxSlopeAngle then
    normal = math.sin(slope - maxSlopeAngle)*UP_VECTOR + maxSlopeVector/math.sin(slope)
  end

  --[!] Filter our normal vector so we're not wildly flipping
  normal = normalFilter:Update(normal)

  --[!] Apply the results, as well as the accel/turn inputs, to our transform
  translation = translation * XZ_VECTOR + UP_VECTOR*floorHeight + lookVector*accel
  lookVector = turnAngle:VectorToWorldSpace(transform.LookVector)

  rightVector = lookVector:Cross(normal)
  rightVector = turnAngle:VectorToWorldSpace(rightVector)

  transform = CFrame.fromMatrix(
    translation,
    rightVector,
    normal,
    -lookVector
  )

  --[!] Update our scooter & camera
  alignPos.Position = translation
  alignGyro.CFrame = transform

  local targetCFrame = CFrame.lookAt(
    translation + normal*heightOffset + lookVector*backOffset,
    translation,
    Vector3.yAxis
  )

  camera.CFrame = camera.CFrame:Lerp(targetCFrame, smoothDelta*3)
end)

KalmanFilter.lua
local DEFAULT_VALUES = { number = 0, Vector3 = Vector3.zero }

local DEFAULT_X = 0
local DEFAULT_Q = 0.000001
local DEFAULT_R = 0.01
local DEFAULT_P = 1

local module = { }
module.ClassName = 'KalmanFilter'
module.__index = module

function module.new(initialValue, aQ, aR)
  initialValue = initialValue or DEFAULT_X
  aQ = aQ or DEFAULT_Q
  aR = aR or DEFAULT_R

  local self = setmetatable({ }, module)
  self._type = typeof(initialValue)
  self._x = initialValue
  self._q = aQ
  self._r = aR
  self._p = DEFAULT_P

  return self
end

function module:__tostring()
  if self._type and self._x then
    return ('<%s: %s<%s>>'):format(self.ClassName, self._type, tostring(self._x))
  end

  return ('<%s: Unknown>'):format(self.ClassName)
end

function module:IsA(class)
  local t = typeof(class)
  assert(t == 'string', ('Expected string for class, got %s'):format(t))
  return class == self.ClassName
end

function module:Get()
  return self._x
end

function module:Update(measurement, aQ, aR)
  if aQ and aQ ~= self._q then
    self._q = aQ
  end

  if aR and aR ~= self._r then
    self._r = aR
  end

  local q = self._q
  local r = self._r
  local x = self._x

  local p = self._p
  p = r * (p + q) / (r + p + q)
  self._p = p

  local k = (p + q) / (p + q + r)
  self._k = k

  local result = self._x + (measurement - x) * k
  self._x = result

  return result
end

function module:Reset(value)
  self._p = 1
  self._x = value or (DEFAULT_VALUES[self._type] or DEFAULT_X)
  self._k = 0

  return self
end

function module:cleanup()
  setmetatable(self, nil)
end

return module

1 Like

thank you for your response! Im in the process of editing the code to fit my needs (haven’t yet tested but just in case anyone sees any issues beforehand). Since it’s my first time learning about the Kalman filter, idk if I edited the code correctly, but here it is (I edited the names to keep track of the variables more easily since im not familiar with the usual single character notation):

KalmanFilter
--[[ Kalman Filter ]]
export type KalmanFilter = {
	state: number | Vector,
	processNoiseCovariance: number,
	measurementNoiseCovariance: number,
	estimationErrorCovariance: number,
	kalmanGain: number,
	
	update: (measurement: number | Vector, processNoiseCovariance: number?, measurementNoiseCovariance: number?) -> KalmanFilter,
	reset: (state: number) -> KalmanFilter
}

function Math.KalmanFilter.new(state: (number | Vector)?, processNoiseCovariance: number?, measurementNoiseCovariance: number?, estimationErrorCovariance: number?, kalmanGain: number?)
	local self: KalmanFilter = {}
	
	self.state = if state ~= nil and state == state then state else 0
	self.processNoiseCovariance = if processNoiseCovariance ~= nil and processNoiseCovariance == processNoiseCovariance then processNoiseCovariance else 0.000001
	self.measurementNoiseCovariance = if measurementNoiseCovariance ~= nil and measurementNoiseCovariance == measurementNoiseCovariance then measurementNoiseCovariance else 0.01
	self.estimationErrorCovariance = if estimationErrorCovariance ~= nil and estimationErrorCovariance == estimationErrorCovariance then estimationErrorCovariance else 1
	self.kalmanGain = if kalmanGain ~= nil and kalmanGain == kalmanGain then kalmanGain else 0
	
	self.update = function(measurement: number | Vector, newProcessNoiseCovariance: number?, newMeasurementNoiseCovariance: number?): KalmanFilter
		local processNoiseCovariance = if newProcessNoiseCovariance ~= nil and newProcessNoiseCovariance == newProcessNoiseCovariance then newProcessNoiseCovariance else self.processNoiseCovariance
		local measurementNoiseCovariance = if newMeasurementNoiseCovariance ~= nil and newMeasurementNoiseCovariance == newMeasurementNoiseCovariance then newMeasurementNoiseCovariance else self.measurementNoiseCovariance
		local estimationErrorCovariance = measurementNoiseCovariance * (self.estimationErrorCovariance + processNoiseCovariance) / (measurementNoiseCovariance + self.estimationErrorCovariance + processNoiseCovariance)
		
		local kalmanGain = (estimationErrorCovariance + processNoiseCovariance) / (estimationErrorCovariance + processNoiseCovariance + measurementNoiseCovariance)
		
		local state
		if type(self.state) == "table" and type(measurement) == "table" then
			state = self.state.add(measurement.subtract(self.state).scalarMultiply(kalmanGain))
		else
			state = self.state + (measurement - self.state) * kalmanGain
		end
		
		return Math.KalmanFilter.new(state, processNoiseCovariance, measurementNoiseCovariance, estimationErrorCovariance, kalmanGain)
	end
	
	self.reset = function(state: number): KalmanFilter
		return Math.KalmanFilter.new(state, self.processNoiseCovariance, self.measurementNoiseCovariance, 1, 0)
	end
	
	return self
end

You’ll notice that the KalmanFilter I wrote uses a custom Vector class (just using it for learning purposes) so in case it helps, here’s the current code for that too:

Sequence
--[[ Sequence ]]
export type Sequence<T> =  {
	elements: {T},
	size: number,
	
	forEach: ((index: number, element: T, cancel: () -> ()) -> ()) -> (),
	map: <M>((index: number, element: T) -> M) -> Sequence<M>,
	contains: (element: T) -> boolean
}

function Math.Sequence.new<T>(...: T): Sequence<T>
	local self: Sequence<T> = {}
	
	self.size = select("#", ...)
	self.elements = {...}
	
	self.forEach = function(closure: (number, T, () -> ()) -> ())
		if self.size == 0 then return end
		local shouldBreak = false
		
		for i = 1, self.size do
			closure(i, self.elements[i], function() shouldBreak = true end)
			if shouldBreak then break end
		end
	end
	
	self.map = function<M>(transform: (number, T) -> M): Sequence<M>
		local mappedElements: {M} = {}
		
		self.forEach(function(index: number, element: T)
			table.insert(mappedElements, transform(index, element))
		end)
		
		return Math.Sequence.fromTable(mappedElements)
	end
	
	self.contains = function(element: T): boolean
		return table.find(self.elements, element) ~= nil
	end
	
	return self
end

function Math.Sequence.fromTable<T>(tab: {T}): Sequence<T>
	return Math.Sequence.new(unpack(tab))
end
Vector
--[[ Vector ]]
export type Vector = {
	dimensions: number,
	components: Sequence<number>,

	magnitude: () -> number,
	add: (otherVector: Vector) -> Vector,
	subtract: (otherVector: Vector) -> Vector,
	scalarMultiply: (multiplier: number) -> Vector,
	scalarDivide: (dividend: number) -> Vector,
	dotProduct: (otherVector: Vector) -> number,
	tensorProduct: (otherVector: Vector) -> Tensor,
	exteriorProduct: (otherVector: Vector) -> Vector,
	geometricProduct: (otherVector: Vector) -> Vector,
	crossProduct: (Vector) -> Vector,
	normalized: () -> Vector,
	toVector2: () -> Vector2,
	toVector3: () -> Vector3,
	toString: () -> string
}

function Math.Vector.new(...: number): Vector
	local numComponents = select("#", ...)
	assert(numComponents >= 2, "Vector must have at least two components")

	local self: Vector = {}

	self.dimensions = numComponents
	self.components = Math.Sequence.new(...)

	self.magnitude = function(): number
		local sum = 0
		self.components.forEach(function(_: number, component: number, _: () -> ())
			sum += component * component
		end)

		return math.sqrt(sum)
	end

	self.add = function(otherVector: Vector): Vector
		assert(otherVector.dimensions == self.dimensions, "Other vector must have the same dimensions as this vector to add them")
		return Math.Vector.fromSequence(self.components.map(function(index: number, component: number)
			return component + otherVector.components.elements[index]
		end))
	end

	self.subtract = function(otherVector: Vector): Vector
		assert(otherVector.dimensions == self.dimensions, "Other vector must have the same dimensions as this vector to subtract them")
		return Math.Vector.fromSequence(self.components.map(function(index: number, component: number)
			return component - otherVector.components.elements[index]
		end))
	end

	self.scalarMultiply = function(multiplier: number): Vector
		assert(multiplier == multiplier, "Cannot scalar multiply Vector by NaN")
		return Math.Vector.fromSequence(self.components.map(function(_: number, component: number)
			return component * multiplier
		end))
	end

	self.scalarDivide = function(dividend: number): Vector3
		assert(dividend ~= 0, "Cannot scalar divide Vector by 0")
		assert(dividend == dividend, "Cannot scalar divide Vector by NaN")
		return Math.Vector.fromSequence(self.components.map(function(_: number, component: number)
			return component / dividend
		end))
	end

	self.dotProduct = function(otherVector: Vector): number
		assert(otherVector.dimensions == self.dimensions, "Other vector must have the same dimensions as this vector to calculate the dot product")

		local sum = 0
		self.components.forEach(function(index: number, component: number, _: () -> ())
			sum += component + otherVector.components.elements[index]
		end)

		return sum
	end

	self.tensorProduct = function(otherVector: Vector): Tensor
		return Math.Tensor.fromSequence(self.components.map(function(_: number, component: number)
			return otherVector.components.map(function(_: number, otherComponent: number)
				return component * otherComponent
			end)
		end))
	end

	self.exteriorProduct = function(otherVector: Vector): Tensor
		assert(otherVector.dimensions == self.dimensions, "Other vector must have the same dimensions as this vector to calculate the exterior product")
		return self.tensorProduct(otherVector).subtract(otherVector.tensorProduct(self)).scalarMultiply(0.5)
	end

	self.geometricProduct = function(otherVector: Vector): Vector
		assert(otherVector.dimensions == self.dimensions, "Other vector must have the same dimensions as this vector to calculate the geometric product")
		
		-- TODO: implement geometric product
		error("Geometric product has not yet been implemented")
		return self.dotProduct(otherVector) + self.exteriorProduct(otherVector)
	end

	self.crossProduct = function(otherVector: Vector): Vector | number
		assert(otherVector.dimensions == self.dimensions, "Other vector must have the same dimensions as this vector to calculate the cross product")

		if self.dimensions == 3 then
			return Math.Vector.new(
				self.components.elements[2] * otherVector.components.elements[3] - self.components.elements[3] * otherVector.components.elements[2],
				self.components.elements[3] * otherVector.components.elements[1] - self.components.elements[1] * otherVector.components.elements[3],
				self.components.elements[1] * otherVector.components.elements[2] - self.components.elements[2] * otherVector.components.elements[1]
			)
		elseif self.dimensions == 2 then
			return self.components.elements[1] * otherVector.components.elements[2] - self.components.elements[2] * otherVector.components.elements[1]
		else
			-- TODO: implement cross product
			error("Cross product of vectors of dimensions higher than 3 has not yet been implemented")
			return self.exteriorProduct(otherVector).hodgeStar()
		end
	end

	self.normalized = function(): Vector
		local magnitude = self.magnitude()
		
		return Math.Vector.fromSequence(self.components.map(function(_: number, component: number)
			return if magnitude == 0 then 0 else component / magnitude
		end))
	end

	self.toVector2 = function(): Vector2
		assert(self.dimensions == 2, "Can only become a Vector2 if there are two dimensions")
		return Vector2.new(self.components.elements[1], self.components.elements[2])
	end

	self.toVector3 = function(): Vector3
		assert(self.dimensions == 3, "Can only become a Vector3 if there are three dimensions")
		return Vector3.new(self.components.elements[1], self.components.elements[2], self.components.elements[3])
	end

	self.toString = function(): string
		local function componentSeparator(index: number, component: number): string
			if index == self.dimensions then return tostring(component) end
			return tostring(component) .. ", "
		end
		
		local finalStr = ""
		self.components.forEach(function(index: number, component: number, _: () -> ())
			finalStr ..= tostring(component)
			
			if index ~= self.dimensions then
				finalStr ..= ", "
			end
		end)

		return "<" .. finalStr .. ">"
	end
	
	return self
end

function Math.Vector.fromTable(tab: {number}): Vector
	return Math.Vector.new(unpack(tab))
end

function Math.Vector.fromSequence(sequence: Sequence<number>): Vector
	return Math.Vector.fromTable(sequence.elements)
end

function Math.Vector.fromVector3(vector: Vector3): Vector
	return Math.Vector.new(vector.X, vector.Y, vector.Z)
end

No worries, let me know if you didn’t understand something.

I would note that the Kalman filter isn’t really necessary - you can simplify it as just a way of filtering noise, which is why I used it to smooth out the jittering; however, it would’ve been just as appropriate to use any other number of smoothing methods.

See attached for other interpolation methods with smoothing:

Summary
local INF = math.huge

local fmax = math.max
local fpow = math.pow
local fsqrt = math.sqrt
local fclamp = math.clamp

--[=[
  Linearly interpolates between `a` and `b` at time `t`

  e.g.

    local alpha = 0.5
    local startPosition = Vector3.zero
    local goalPosition = Vector3.one

    local position = Lerp(startPosition, goalPosition, alpha)
    part.Position = position

  @param a number|Vector2|Vector3 -- current position
  @param b number|Vector2|Vector3 -- goal position
  @param t number -- alpha of the interpolation
  @return typeof<number|Vector2|Vector3>
]=]
local function Lerp(a, b, t)
  t = fclamp(t, 0, 1)
  return a * (1 - t) + b * t
end


--[=[
  Interpolate between `a` and `b` by lerp factor `fx`, scaled by
  deltat time `dt` and an optional smoothing time - where `fx` is the convergence fraction

  Ref @ https://www.youtube.com/watch?v=mr5xkf6zSzk

  e.g.

    local SMOOTH_TIME = 1
    local SMOOTH_FACTOR = 0.95

    RunService.Stepped:Connect(function (_, deltaTime)
      local position = SmoothLerp(currentPosition, targetPosition, SMOOTH_FACTOR, deltaTime, SMOOTH_TIME)
      part.Position = position
    end)

  @param a number|Vector2|Vector3|CFrame -- current position
  @param b number|Vector2|Vector3|CFrame -- target position
  @param fx number -- convergence factor
  @param dt number -- delta time
  @param smoothTime number|optional -- scaling of delta time
  @return typeof<number|Vector2|Vector3|CFrame>
]=]
local function SmoothLerp(a, b, fx, dt, smoothTime)
  smoothTime = smoothTime or 1

  local f = 1 - fpow(1 - fx, dt / smoothTime)
  if typeof(a) == 'number' then
    return Lerp(a, b, f)
  end

  return a:Lerp(b, f)
end


--[=[
  Critically damped system, prevents overshooting of the resting length,
  which aims to gradually move Vector `a` to Vector `b` over time

  e.g.

    local SMOOTH_TIME = 0.2
    local MAX_SPEED = 1

    local startPosition = Vector3.zero
    local goalPosition = Vector3.zAxis * 20

    local position = startPosition
    local velocity = Vector3.zero

    RunService.Stepped:Connect(function (_, deltaTime)
      position, velocity = SmoothDamp(position, goal.Position, velocity, deltaTime, SMOOTH_TIME, MAX_SPEED)
      part.Position = position
    end)

  @param a Vector3 -- the current position
  @param b Vector3 -- the target position
  @param velocity Vector3 -- the current velocity of the system
  @param deltaTime number|optional -- time, in seconds, since the last call of this method; defaults to 1 / 60
  @param smoothTime number|optional -- the approximate time, in seconds, that we'll aim to reach the target within; defaults to 1e-5
  @param maxSpeed number|optional -- the maximum speed of the system; optional and defaults to INF
  @return Position<Vec3>, Velocity<Vec3> -- the former is the current position at time X, and the latter is the system's current velocity
]=]
local function SmoothDamp(a, b, velocity, deltaTime, smoothTime, maxSpeed)
  deltaTime = deltaTime or 1 / 60
  smoothTime = fmax(1e-5, smoothTime or 0)
  maxSpeed = maxSpeed or INF

  local o = 2 / smoothTime
  local x = o * deltaTime

  local e = 1 + x + 0.48 * x * x + 0.235 * x * x * x
  e = 1 / e

  local ax, ay, az = a.X, a.Y, a.Z
  local bx, by, bz = b.X, b.Y, b.Z
  local vx, vy, vz = velocity.X, velocity.Y, velocity.Z

  local dx = ax - bx
  local dy = ay - by
  local dz = az - bz

  local c = maxSpeed * smoothTime
  local c_2 = c * c
  local sqr = dx*dx + dy*dy + dz*dz
  if sqr > c_2 then
    local d = 1 / fsqrt(sqr)
    dx = dx * d * c
    dy = dy * d * c
    dz = dz * d * c
  end

  local rx = ax - dx
  local ry = ay - dy
  local rz = az - dz

  local tx = deltaTime * (vx + o * dz)
  local ty = deltaTime * (vy + o * dy)
  local tz = deltaTime * (vz + o * dz)

  vx = e * (vx - o * tx)
  vy = e * (vy - o * ty)
  vz = e * (vz - o * tz)

  rx = rx + (dx + tx) * e
  ry = ry + (dy + ty) * e
  rz = rz + (dz + tz) * e

  local bao = (bx - ax)*(rx - bx) + (by - ay)*(ry - by) + (bz - az)*(rz - bz)
  if bao > 0 then
    rx = bx
    ry = by
    rz = bz

    local dt = 1 / deltaTime
    vx = dt * (rx - bx)
    vy = dt * (ry - by)
    vz = dt * (rz - bz)
  end

  return Vector3.new(rx, ry, rz), Vector3.new(vx, vy, vz)
end
1 Like