3d Inverse kinematics

[angros47]

I tried porting my example of inverse kinematics to 3d, using OpenB3D. Looks like it works (control the sphere with cursor keys and A/Z)

Code: Select all

#include "openb3d.bi"

#macro atan2deg( a, b )
	atan2(a,b)*180/3.1415926535897
#endmacro

const maxjoints=2

screen 18, 32, , &h10002	

	
Graphics3d 640,480,32,1,1


var camera=createcamera(0)
moveentity camera,0,0,-10

var target=createsphere(16)
entitycolor target,255,0,0

dim key as string


dim joint(maxjoints) as any ptr
var j=createcone()
fitmesh j,-.2,0,-.2,.4,2,.4
rotatemesh j,-90,0,0

joint(0)=copyentity(j)
for i as integer=1 to maxjoints
	joint(i)=copyentity(j,joint(i-1))
	positionentity joint(i),0,0,2
next

freeentity j

var endpoint=createsphere(16,joint(maxjoints))
scalemesh endpoint,.2,.2,.2
positionentity endpoint,0,0,2


do

	key=inkey
        if key="a" then Moveentity target,0,0,1
        if key="z" then Moveentity target,0,0,-1

        if key=chr(255)+"H" then Moveentity target,0,1,0
        if key=chr(255)+"P" then Moveentity target,0,-1,0
        if key=chr(255)+"M" then Moveentity target,1,0,0
        if key=chr(255)+"K" then Moveentity target,-1,0,0





	for i as integer=maxjoints to 0 step -1
		if i>0 then entityparent joint(i),0,1

		dim as single endx=entityx(joint(i),1)-entityx(endpoint,1)
		dim as single endy=entityy(joint(i),1)-entityy(endpoint,1)
		dim as single endz=entityz(joint(i),1)-entityz(endpoint,1)

		dim as single targetx=entityx(joint(i),1)-entityx(target,1)
		dim as single targety=entityy(joint(i),1)-entityy(target,1)
		dim as single targetz=entityz(joint(i),1)-entityz(target,1)

		dim as single end_pitch=atan2deg(-endy, sqr(endz*endz+endx*endx))
		dim as single end_yaw=atan2deg(endx, -endz)

		dim as single target_pitch=atan2deg(-targety, sqr(targetz*targetz+targetx*targetx))
		dim as single target_yaw=atan2deg(targetx, -targetz)

		turnentity joint(i),0,-end_yaw,0,1
		turnentity joint(i),-end_pitch,0,0,1

		turnentity joint(i),target_pitch,0,0,1
		turnentity joint(i),0,target_yaw,0,1

		if i>0 then 
			entityparent joint(i),joint(i-1),1
			positionentity joint(i),0,0,2
		end if
	next

	updateworld 1
	renderworld



sleep 1
	flip
loop until key=chr(27)

[Rachowasun]

I tried porting my example of inverse kinematics to 3d, using OpenB3D. Looks like it works (control the sphere with cursor keys and A/Z)

Code: Select all

#include "openb3d.bi"

#macro atan2deg( a, b )
	atan2(a,b)*180/3.1415926535897
#endmacro

const maxjoints=2

screen 18, 32, , &h10002	

	
Graphics3d 640,480,32,1,1


var camera=createcamera(0)
moveentity camera,0,0,-10

var target=createsphere(16)
entitycolor target,255,0,0

dim key as string


dim joint(maxjoints) as any ptr
var j=createcone()
fitmesh j,-.2,0,-.2,.4,2,.4
rotatemesh j,-90,0,0

joint(0)=copyentity(j)
for i as integer=1 to maxjoints
	joint(i)=copyentity(j,joint(i-1))
	positionentity joint(i),0,0,2
next

freeentity j

var endpoint=createsphere(16,joint(maxjoints))
scalemesh endpoint,.2,.2,.2
positionentity endpoint,0,0,2


do

	key=inkey
        if key="a" then Moveentity target,0,0,1
        if key="z" then Moveentity target,0,0,-1

        if key=chr(255)+"H" then Moveentity target,0,1,0
        if key=chr(255)+"P" then Moveentity target,0,-1,0
        if key=chr(255)+"M" then Moveentity target,1,0,0
        if key=chr(255)+"K" then Moveentity target,-1,0,0





	for i as integer=maxjoints to 0 step -1
		if i>0 then entityparent joint(i),0,1

		dim as single endx=entityx(joint(i),1)-entityx(endpoint,1)
		dim as single endy=entityy(joint(i),1)-entityy(endpoint,1)
		dim as single endz=entityz(joint(i),1)-entityz(endpoint,1)

		dim as single targetx=entityx(joint(i),1)-entityx(target,1)
		dim as single targety=entityy(joint(i),1)-entityy(target,1)
		dim as single targetz=entityz(joint(i),1)-entityz(target,1)

		dim as single end_pitch=atan2deg(-endy, sqr(endz*endz+endx*endx))
		dim as single end_yaw=atan2deg(endx, -endz)

		dim as single target_pitch=atan2deg(-targety, sqr(targetz*targetz+targetx*targetx))
		dim as single target_yaw=atan2deg(targetx, -targetz)

		turnentity joint(i),0,-end_yaw,0,1
		turnentity joint(i),-end_pitch,0,0,1

		turnentity joint(i),target_pitch,0,0,1
		turnentity joint(i),0,target_yaw,0,1

		if i>0 then 
			entityparent joint(i),joint(i-1),1
			positionentity joint(i),0,0,2
		end if
	next

	updateworld 1
	renderworld



sleep 1
	flip
loop until key=chr(27)

I ran into the same challenge recently while trying to get inverse kinematics working in 3D with OpenB3D. At first, everything seemed fine, but then I noticed weird joint snapping and misalignment when moving the target. After some digging, I found that my issue was how I handled entity parenting, resetting it every loop iteration caused unexpected behavior. Once I adjusted it to set only when needed, things started working much more smoothly. Another thing that helped was carefully checking my atan2 calculations. Sometimes, unexpected flips happened due to angle misinterpretation. Maybe try logging the yaw and pitch values separately to see if they make sense?

[angros47]

That issue happens because internally, every entity has two matrices: one is the rotation matrix, that contains information about relative pitch, yaw, and roll, and the other is the transformation matrix used when the entity is rendered, that contains information about absolute rotation, position and scaling.

EntityParent reconstructs the rotation matrix by taking the transform matrix and removing translation and scaling: it works, but little rounding errors are inevitable. Usually, those errors are irrelevant, but if the operation is iterated too many times, they become cumulative

The command RotateEntity rebuilds the rotation matrix from scratch, so the issue can be fixed perfectly by adding after the line

Code: Select all

		if i>0 then entityparent joint(i),0,1

a new line:

Code: Select all

rotateentity joint(i),entitypitch(joint(i)), entityyaw(joint(i)), entityroll(joint(i))

At first glance, it looks pointless (it reads pitch, yaw and roll and immediately applies it back, changing nothing) but actually it fixes the issue. I didn't include it in the first version because it is computationally expensive (heavy call to trigonometric functions)

[angros47]

Ok, I found the cause of the issue, and most important, a way to fix it. The rotation matrix just needs to be orthonormalized (basically, made sure that it contains no scaling, and that the three dimensions are orthogonal). It can be achieved by the algorithm of Gram-Schmidt

https://en.wikipedia.org/wiki/Gram%E2%8 ... dt_process

With the help of CoPilot, I elaborated the code:

Code: Select all

	void ToRotMat() {
		// Step 1: Normalize the first column vector
		float length = sqrt(grid[0][0]*grid[0][0]+grid[0][1]*grid[0][1]+grid[0][2]*grid[0][2]);
		grid[0][0]=grid[0][0]/length;
		grid[0][1]=grid[0][1]/length;
		grid[0][2]=grid[0][2]/length;

		// Step 2: Orthogonalize the second column vector with respect to the first
		float projection[3];
		float dot_product=grid[1][0]*grid[0][0]+grid[1][1]*grid[0][1]+grid[1][2]*grid[0][2];
		projection[0]=dot_product * grid[0][0];
		projection[1]=dot_product * grid[0][1];
		projection[2]=dot_product * grid[0][2];

		grid[1][0]=grid[1][0]-projection[0];
		grid[1][1]=grid[1][1]-projection[1];
		grid[1][2]=grid[1][2]-projection[2];

		//normalize(grid[1]);
		length = sqrt(grid[1][0]*grid[1][0]+grid[1][1]*grid[1][1]+grid[1][2]*grid[1][2]);
		grid[1][0]=grid[1][0]/length;
		grid[1][1]=grid[1][1]/length;
		grid[1][2]=grid[1][2]/length;

		// Step 3: Orthogonalize the third column vector with respect to the first and second
		dot_product=grid[2][0]*grid[0][0]+grid[2][1]*grid[0][1]+grid[2][2]*grid[0][2];
		projection[0] = dot_product * grid[0][0];
		projection[1] = dot_product * grid[0][1];
		projection[2] = dot_product * grid[0][2];

		grid[2][0]=grid[2][0]-projection[0];
		grid[2][1]=grid[2][1]-projection[1];
		grid[2][2]=grid[2][2]-projection[2];

		dot_product=grid[2][0]*grid[1][0]+grid[2][1]*grid[1][1]+grid[2][2]*grid[1][2];
		projection[0] = dot_product * grid[1][0];
		projection[1] = dot_product * grid[1][1];
		projection[2] = dot_product * grid[1][2];

		grid[2][0]=grid[2][0]-projection[0];
		grid[2][1]=grid[2][1]-projection[1];
		grid[2][2]=grid[2][2]-projection[2];

		//normalize(grid[2]);
		length = sqrt(grid[2][0]*grid[2][0]+grid[2][1]*grid[2][1]+grid[2][2]*grid[2][2]);
		grid[2][0]=grid[2][0]/length;
		grid[2][1]=grid[2][1]/length;
		grid[2][2]=grid[2][2]/length;


		//remove translation
		grid[3][0] = 0;
		grid[3][1] = 0;
		grid[3][2] = 0;

		// The right column vector of the matrix should always be [ 0 0 0 1 ]
		// in most cases. . . you don't need this column at all because it'll
		// never be used in the program, but since this code is used with GL
		// and it does consider this column, it is here.
		grid[0][3] = 0;
		grid[1][3] = 0;
		grid[2][3] = 0;
		grid[3][3] = 1;
	}

it will be included in next version