How do I convert midpoint angle to vector3 position?

[sanjay2003 (Sanjay)]

Hey, I’ve been trying to figure out how to convert an angle(solved) from two lengths and convert that to
a Vector3 position in 3D space. So far, I have no idea how to go about this.

I’ve been trying to make a method relating to Inverse Kinematics. It takes two points, draws segments(via midpoint) and uses a defined midpoint to calculate Size.Z for each segment.

For whatever reason, I can’t seem to figure out a way to convert an angle, that I solve for using Law of Cosines, to a valid vector3 position(where the midpoint should be). Typically, IK is not solved like this but, I figured that I could try it out. The way I set it up is that there’s two segments. Segment 1 spans from start(Shoulder) to the midpoint. Segment 2 spans from the midpoint to goal(end-effector). In theory, I should be able to move the midpoint to get the correct position of segment 2. However, oddly enough I have no idea to determine the midpoint’s vector3 position after the end-effector has moved.
Shown in the video below, I know the angle that both segments should be making, but I have no clue how to produce that programically.

Programically, it looks something like this:

-- Midpoint Formula
midpoint.Position = Vector3.new((start.Position.X+goal.Position.X)/2,(start.Position.Y+goal.Position.Y)/2,(start.Position.Z+goal.Position.Z)/2)

-- Distance Formula
distance1 = math.sqrt((midpoint.Position.X-start.Position.X)^2+(midpoint.Position.Y-start.Position.Y)^2+(midpoint.Position.Z-start.Position.Z)^2)/2

-- Segment Sizing
seg1.Size = Vector3.new(1.3,1.3,(distance1-1.3)/2)
seg2.Size = Vector3.new(1.3,1.3,(distance1-1.3)/2)
-- Code on this line is basic cframe to position segments to start -> midpoint & midpoint -> goal respectively

-- v1 = Segment 1, v2 = Segment 2
-- Shoulder = start, Goal = goal
local a = tonumber(math.sqrt(v1.Position.X^2+v1.Position.Y^2+v1.Position.Z^2))
local b = tonumber(math.sqrt(v2.Position.X^2+v2.Position.Y^2+v2.Position.Z^2))
local c = (shoulder.Position-goal.Position).magnitude
local A = math.acos((-a^2+b^2-c^2)/(2*b*c))
local C = math.acos((a^2+b^2-c^2)/(2*a*b))

Here’s what I have so far:
https://streamable.com/5j7dd

Just a small disclaimer, I’m aware that this is not the most conventional way to do this but, so far up until this point, it’s been working for me, could be totally wrong, but works. I’m not entirely sure if it qualifies as IK. Anywho, I appreciate those who choose to respond.

[goldenstein64 (Shoop)]

I’m going to simplify/style your code just for the sake of understanding it better, don’t mind me:

-- Midpoint Formula
midpoint.Position = (start.Position + goal.Position)/2

-- Distance Formula
distance1 = (midpoint.Position - start.Position).Magnitude

-- Segment Sizing
local size = Vector3.new(1.3, 1.3, (distance1 - 1.3)/2)
seg1.Size = size
seg2.Size = size
-- Code on this line is basic cframe to position segments to start -> midpoint & midpoint -> goal respectively

-- v1 = Segment 1, v2 = Segment 2
-- Shoulder = start, Goal = goal
local a = v1.Magnitude
local b = v2.Magnitude
local c = (shoulder.Position - goal.Position).Magnitude

-- Law of Cosines
local A = math.acos((-a^2+b^2-c^2)/(2*b*c))
local C = math.acos((a^2+b^2-c^2)/(2*a*b))

By the looks of it, you are trying to solve for B, since that seems to be the only angle you are missing, or am I wrong?

I think your law of cosines formula is wrong? Wikipedia says you would only have to subtract one term:

• the angles of a triangle if one knows the three sides:

For anyone curious, the weird Y (actually gamma) symbol represents the angle opposite of side c, otherwise known as uppercase C in this code block.

A = math.acos((b^2 + c^2 - a^2) / (2 * b * c))
C = math.acos((a^2 + b^2 - c^2) / (2 * a * b))

Your question is not all that clear, you say you’re trying to take the angle between two lengths and convert it into a new position, but there was no info given on how you want this conversion to work.

[sanjay2003 (Sanjay)]

Thanks for formatting my code. haha

@goldenstein64

I think your law of cosines formula is wrong? Wikipedia says you would only have to subtract one term:

My mistake. I forgot to catch that. Much appreciate.
However, to get angle A, the equation should be: b^2+c^2-a^2/2bc or lua equivalent of:

math.acos((b^2+c^2-a^2)/(2*b*c))

I solved all three sides a,b,c and angles A & C. However, even if I solve angle B, how do I relate that to a vector3 where my midpoint should be?

[goldenstein64 (Shoop)]

Couldn’t you just use your midpoint formula from before to create your new midpoint?

midpoint = (shoulder.Position + goal.Position)/2

I should point out that this creates a new Vector3 like your original formula.

I probably have the wrong idea here, but that’s because I don’t know what you’re asking.

[sanjay2003 (Sanjay)]

I’ve tried that and as a result it produces this where the midpoint is constricted to that singular point.

However, I want it to look like this(I set midpoint to transparency = 1):

For some odd reason, it’s only moving linear even when it’s supposed to make this bend. The angle those two segments in that picture is Angle B or:

math.acos((c^2+a^2-b^2)/(2*c*a))

However, how do I make it so my midpoint position presents Angle B?

[goldenstein64 (Shoop)]

I think I read your previous code a little wrong, I forgot to index the Position property on midpoint, so it just replaced the original object representing the midpoint with a Vector3 value.

This should be the correct code:

midpoint.Position = (shoulder.Position + goal.Position)/2

If this doesn’t work, try lerping it instead:

midpoint.Position = shoulder.Position:Lerp(goal.Position, 0.5)

If you want to instead correct the goal position based on shoulder and midpoint, you can lerp twice the distance between those two points instead:

goal.Position = shoulder.Position:Lerp(midpoint.Position, 2)

woah I replied two hours later

[sanjay2003 (Sanjay)]

All of those don’t place midpoint to represent angle B. Those are linear positions.

Trying to achieve:

Angle B represents the elbow.

[goldenstein64 (Shoop)]

Quick question: do Segment 1 and Segment 2 have to be the same length?

[sanjay2003 (Sanjay)]

Sure. However, when need be, midpoint can be offsetted.
As of now, they’re intended to be the same length.

[goldenstein64 (Shoop)]

local dist = goal.Position - shoulder.Position
local cf = CFrame.new(shoulder.Position, goal.Position) * CFrame.new(0,0,dist.Magnitude/2)

local B = math.pi/4 -- angle in radians

local axis = 0 -- axis of rotation to determine where to place midpoint

cf = cf * CFrame.Angles(0,0,axis) * CFrame.new(0,(math.cos(B)/math.sin(B)) * dist.Magnitude,0)

midpoint.Position = cf.Position

Could this work?

Edit: just edited the second cf assignment

Edit 2: just edited the second cf assignment again

Edit 3: It’s probably still wrong, the second cf assignment is stumping me

[sanjay2003 (Sanjay)]

No. You have to remember that segment 1 is attached to goal → midpoint and segment 2 is attached to start → midpoint. For example this is how it looks like right now:
https://streamable.com/26b84

[goldenstein64 (Shoop)]

@sanjay2003

For whatever reason, I can’t seem to figure out a way to convert an angle, that I solve for using Law of Cosines, to a valid vector3 position(where the midpoint should be). Typically, IK is not solved like this but, I figured that I could try it out. The way I set it up is that there’s two segments. Segment 1 spans from start(Shoulder) to the midpoint. Segment 2 spans from the midpoint to goal(end-effector). In theory, I should be able to move the midpoint to get the correct position of segment 2. However, oddly enough I have no idea to determine the midpoint’s vector3 position after the end-effector has moved.
Shown in the video below, I know the angle that both segments should be making, but I have no clue how to produce that programically.

• “I can’t seem to figure out a way to convert an angle […] to a valid vector3 position.”

How do you want to convert this position? You don’t want a way to make a brand new midpoint between the start and goal, post #4, and you don’t need a way to make a midpoint if you already know the angle between the segments, post #10.

• “In theory, I should be able to move the midpoint to get the correct position of segment 2.”

How does segment 2 get this “correct position”? Would it just be the midpoint between the previous midpoint and the goal? You could even resize the segment to represent this accurately.

• “However, […] I have no idea [how] to determine the midpoint’s […] position after the end-effector has moved.”

What do you want to happen when the end-effector (aka goal) moves? Do you want the midpoint (and maybe the start) to move with the effector?
The simplest solution is applying the same movement to the other points, but that wouldnt be very intuitive at all.
Inverse kinematics will probably be creating a sort of chain link behavior I would assume. Letting only the midpoint move leads to post #10, letting the start move as well is where you would need to research forward kinematics.

• “I know the angle that both segments should be making, but I have no clue how to produce that [programmatically].”

This is what I was trying to solve in post #10, because you knew the angle, and that’s how you would produce it, given the 2 segments are the same length, as long as you solve for angle B and recalculate segment CFrames/sizes.

[sanjay2003 (Sanjay)]

@goldenstein64

How does segment 2 get this “correct position”? Would it just be the midpoint between the previous midpoint and the goal?

When you move goal(end-effector) towards the shoulder, it should bend. That’s where Angle B should come in handy. The two segments, in that state, should represent Angle B. However, how do you set/update/get the position that midpoint is supposed to be at that given point?

@goldenstein64

What do you want to happen when the end-effector (aka goal) moves? Do you want the midpoint (and maybe the start) to move with the effector?

Whatever direction the goal(end-effector) moves, midpoint should move with it. For instance, an arm, hold out your arm. Ok. Hold it straight. Your arm represents the model we’re trying to solve. Your hand represents goal(end-effector). Your elbow represents midpoint. Your shoulder represents shoulder. The distance from your shoulder to your elbow represents segment 1. In this particular case(since both segments are the same length), the distance from your hand to your elbow represents segment 2. What happens when you extend your hand out as far as you can? It stays linear and stiff. This is technically a constraint but, those haven’t been applied yet. What happens when you bring your hand close to you? Your elbow shifts position. Now, bring your hand half the distance. You see it’s bent. If you were to draw an imaginary line from your shoulder to your and, you’d have a triangle where the angle closest to your elbow is Angle B.

The main issue here is that, I know what angle the segments should be making that current time but, I don’t know how to position midpoint to reflect this. As of now, no matter how I move goal(end-effector), it’s strictly linear(stiff, straight) and each segment points to the direction of goal(end-effector).

[IdiomicLanguage]

Nice Problem.

So, we have a known start point s, a known end point e, a known length from the start to the joint (line 1), a known length from the joint to the end (line 2), and an unknown joint point j. Part of this problem is that the solution is not constrained enough. There are many joint points that satisfy these parameters. It will result in the equation for a circle defining all the possible solutions for the joint point. To fix this I’ll add a known unit direction q in which we want the elbow to go which is orthogonal to the line between the start and the goal.

Properly constrained, let’s solve it! Here are the equations we got:

1. We want to find the joint in a given direction. By converting all of our known parameters into the below definitions for X and Y, we will automatically solve for the solutions in that space.

x = |se|
y = q

2. Distance d from start to end is equal to the lengths of the two lines projected onto the lines between the start and end. Lets imagine the possible positions of each arm as spheres of the arm length radius around the start and end points. Using this approach, our solution space is the circle of intersection of these spheres intersecting the plane defined above, yielding two points. We want the point in the desired direction.

s1: x^2 + y^2 = ||sj||^2
s2: (x - d)^2 + y^2 = ||je||^2

s1 = s2

Solving for x:

(x - d)^2 + (||sj||^2 - x^2) = ||je||^2
-- after some quadratic equation magic:
x = (d^2 - ||je||^2 + ||sj||^2) / (2 * d)

Solving for y using x:

x^2 + y^2 = ||sj||^2
y^2 = ||sj||^2 - x^2
y = (||sj||^2 - x^2)^0.5

Since taking the square root yields both a positive and negative value, we need to decide which to use. In this case it is the distance to go in the desired direction for the elbow, so we pick the positive value for y.

To put this all into code:

-- a and b are the lengths from start to joint and joint to goal respectively.
-- dir is the desired direction of the elbow, must be orthogonal to the line
-- and a unit vector.
local function getJoint(start, goal, a, b, dir)
    local line = goal - start
    local d = line.Magnitude
    local x = (d^2 - b^2 + a^2) / (2 * d)
    local y = math.sqrt(a^2 - x^2)
    return x * line.Unit + y * dir
end

(I’d recommend the article here to learn more about circle-circle intersections.)

[sanjay2003 (Sanjay)]

Here’s the what I have written in my codebase using your snippet that you provided.

function anglesSan(v1,v2,shoulder,goal)
	local a = v1.Position.magnitude --math.sqrt((v1.Position.X^2+v1.Position.Y^2+v1.Position.Z^2))
	local b = v2.Position.magnitude --math.sqrt((v2.Position.X^2+v2.Position.Y^2+v2.Position.Z^2))
	local c = (shoulder.Position-goal.Position).magnitude
	local A = math.acos((b^2+c^2-a^2)/(2*b*c))
	local C = math.acos((a^2+b^2-c^2)/(2*a*b))
	local B = math.acos((c^2+a^2-b^2)/(2*c*a))
	--print(a,b,c,A,C,B)
	
	local function getJoint(start,goal,a,b,dir)
		local line = (goal.Position-start.Position)
		local d = line.Magnitude
		local x = (d^2-b^2+a^2)/(2*d)
		local y = math.sqrt(a^2-x^2)
		return x*line.Unit+y*dir
	end
	
	return getJoint(start,goal,a,b,goal.CFrame.lookVector.Unit)
end

midpoint.Position = angleSan(seg1,seg2,start,goal)

It’s very well possible that I could be doing this very wrong, but as of now, this is what is produced when I hit play:
https://streamable.com/0eeqt

Much appreciated for your response. :I apologize in advance if this is way off.

[sanjay2003 (Sanjay)]

Bumpidy, bumpidy, bumpidy, bump.

[IdiomicLanguage]

I can’t see the videos you post btw. From the still shot at the start it seems like the who arms are not connecting to any point, when if my code returned a single point then both arms should connect to it. Maybe you can post the video somewhere else or describe the issue?

Edit: is that actually the elbow way down near the floor?

[IdiomicLanguage]

I opened up studio to test it and made this script. It seems to be working correctly:

local function makePoint(pos)
	local part = Instance.new("Part")
	part.Anchored = true
	part.Size = Vector3.new(1, 1, 1)
	part.Shape = Enum.PartType.Ball
	part.CFrame = CFrame.new(pos)
	part.Parent = workspace
	return part
end

local function makeLine(p1, p2)
	local part = Instance.new("Part")
	part.Anchored = true
	part.Size = Vector3.new(0.2, 0.2, (p1 - p2).Magnitude)
	part.CFrame = CFrame.new((p1 + p2)/2, p2)
	part.Parent = workspace
	return part
end

local function getJoint(start,goal,a,b,dir)
	local line = goal - start
	local d = line.Magnitude
	local x = (d^2 - b^2 + a^2) / (2 * d)
	local y = math.sqrt(a^2 - x^2)
	return x * line.Unit + y * dir
end

local up = Vector3.new(0, 1, 0)
local start = Vector3.new(0, 0, 0)
local goal = Vector3.new(0, 0, 5)
local a = 3
local b = 4
local joint = getJoint(start, goal, a, b, up)
makePoint(start)
makePoint(goal)
makePoint(joint)
makeLine(start, joint)
makeLine(joint, goal)

print(a, (start - joint).Magnitude) --> 3 3
print(b, (goal - joint).Magnitude) --> 4 4

I couldn’t see anything wrong in the code you posted, but I do see lots of leftover variables from the old method you guys were trying. Knowing this method works, maybe cleaning up the implementation will help.

Edit: this version of the getJoint function checks some of the assumptions I mentioned in the solution.

local function getJoint(start, goal, a, b, dir)
	local line = goal - start
	
	if dir.Magnitude ~= 1 then
		warn 'desired direction is not a unit vector'
	end
	if line:Dot(dir) ~= 0 then
		warn 'desired direction is not orthogonal to the line between the start and goal'
	end
	
	local d = line.Magnitude
	local x = (d^2 - b^2 + a^2) / (2 * d)
	local y = math.sqrt(a^2 - x^2)
	return x * line.Unit + y * dir
end

[sanjay2003 (Sanjay)]

Ah, https://streamable.com/0eeqt should be the correct video URL. Not sure what happened.
The yellow is the elbow, yes.

[sanjay2003 (Sanjay)]

I’ve simplified my code(w/ the added additions you provided) to this:

-- IK Test
local runService = game:GetService("RunService")
local model = script.Parent
local start = model.start
local goal = model.goal
local midpoint = model.midpoint

-- Edit: Both are same size as start & goal and exist outside the model. Size and position is changed regardless. You can have any part named “seg1” and “seg2” in workspace and this should work as intended. Forgot to add this to the picture. 
local seg1,seg2 = game.Workspace.seg1,game.Workspace.seg2


-- Midpoint Formula ((x1+x2)/2,(y1+y2)/2,(z1+z2)/2) -> Vector3
-- Initial Segment Lengths(direction,magnitude)
	-- Magnitude = size
	-- Direction = length

-- distance between start & midpoint
distance1 = math.sqrt((midpoint.Position.X-start.Position.X)^2+(midpoint.Position.Y-start.Position.Y)^2+(midpoint.Position.Z-start.Position.Z)^2)/2

seg1.Size = Vector3.new(1.3,1.3,(distance1-1.3)/2)
seg2.Size = Vector3.new(1.3,1.3,(distance1-1.3)/2)
seg1.CFrame = CFrame.new(start.Position,midpoint.Position)*CFrame.new(0,0,-1.3*3)
seg2.CFrame = CFrame.new(goal.Position,midpoint.Position)*CFrame.new(0,0,-1.3*3)




-- Dynamically calculate the midpoint
function anglesSan(v1,v2,shoulder,goal)
	local a = v1.Position.magnitude --math.sqrt((v1.Position.X^2+v1.Position.Y^2+v1.Position.Z^2))
	local b = v2.Position.magnitude --math.sqrt((v2.Position.X^2+v2.Position.Y^2+v2.Position.Z^2))
	local c = (shoulder.Position-goal.Position).magnitude
	local A = math.acos((b^2+c^2-a^2)/(2*b*c))
	local C = math.acos((a^2+b^2-c^2)/(2*a*b))
	local B = math.acos((c^2+a^2-b^2)/(2*c*a))
	
	local function getJoint(start,goal,a,b,dir)
		local line = (goal.Position-start.Position)
		if dir.Magnitude ~= 1 then
			warn("Desired direction is not a unit vector")
		end
		if line:Dot(dir) ~= 0 then
			warn("Desired direction is not orthogonal to the line between the start and goal")
		end
		local d = line.magnitude
		local x = (d^2-b^2+a^2)/(2*d)
		local y = math.sqrt(a^2-x^2)
		return x*line.Unit+y*dir
	end
	
	
	
	return getJoint(shoulder,goal,a,b,goal.CFrame.lookVector.Unit)
end

runService.Heartbeat:connect(function(step)
	midpoint.Position = anglesSan(seg1,seg2,start,goal)
	warn("Midpoint Position: ",midpoint.Position)
	seg1.CFrame = CFrame.new(start.Position,midpoint.Position)*CFrame.new(0,0,-1.3*3)
	seg2.CFrame = CFrame.new(goal.Position,midpoint.Position)*CFrame.new(0,0,-1.3*3)
end)

However, I’ve noticed that the same issue does still apply. I may be missing something here.
Here’s how my parts are setup so, you can possibly test what I mean:

-- Position of parts
-- Different colors to for visual purposes
start.Position = Vector3.new(6.53, 12.5, -10.33)
goal.Position = Vector3.new(-8.47, 12.5, -10.33)
midpoint.Position = Vector3.new(-27.97, 12.5, -10.33) -- Initial position before changes

-- Size of parts
start.Size = Vector3.new(1.3, 1.3, 1.3)
goal.Size = Vector3.new(1.3, 1.3, 1.3)
midpoint.Size = Vector3.new(1.3, 1.3, 1.3)

The script called “Test” contains the larger code snippet in this post. Hopefully, this can help convey what I’m dealing with. The goal of this is to allow this segment to act as an arm. Again, thanks for the follow ups. Much appreciated.