Math Help needed: Polevector for Ik System

Blitz3D Forums/Blitz3D Programming/Math Help needed: Polevector for Ik System

Danny

(Posted 2005) [#1]

I'm crap at trigonometry and have been trying to figure this one out for weeks.

(For those more familiar with character animation,
I've got the basic maths working for an Inverse Kinematics system for Blitz,
but I can't seem to figure out the maths for a pole-vector).

I have 3 pivot entities P1, P2 and P3 in 3d space.
These pivots are not connected, and could be anywhere basically :)
These three pivots create an imaginary plane in 3d space (see blue rectangles in image).

I then have 2 entities, let's say two cylinders, that have a FIXED length (see the yellow & magenta lines).
The yellow one has a fixed length of 0.5, and is anchored at Pivot1
The Magenta one has a fixed length of 0.7 and is anchored at Pivot3

What I'm trying to do is to 'reconstruct' the triangle P1-P4-P3, that is within the plane/space of the 2d blue rectangle (that's oriented in whatever angle in 3D).

So I need to know either the XYZ coordinate of P4 (the green dot) OR the Pitch/Yaw/Roll angles of the yellow / magenta cylinders.

In the image you'll see two situation where the 'imaginary plane' created by p1,p2 and p3 is of a different size & orientation in 3d. But the yellow and magenta lines still have the same exact length.

I hope this makes sense and is in any way clear. I'm working on an IK Animation system and got pretty far with procedural animation, but I need to solve this problem to be able to control a character's elbows for arm-animations and to control their knees / leg orientation for leg-animations like a walk, jump, crouch or run cycle.

Any help would be GREATLY appreciated!

Thanks,
Danny

BlackJumper

(Posted 2005) [#2]

This might get you started...

4 Coplanar points

| x1  y1  z1  1|
| x2  y2  z2  1|
| x3  y3  z3  1| = 0
| x4  y4  z4  1|

Since you know that the vectors for P1 + Y = P4 and simultaneously P3 + M = P4 you can equate these and (I think) solve for the x,y,z of P4.

Here Y is the vector representing the Yellow line and M is the vector for the Magenta line.

the above matrix becomes

| x1      y1     z1     1|
| x2      y2     z2     1|
| x3      y3     z3     1| = 0
| x1+Yx   y1+Yy  z1+Yz  1|

and

| x1      y1     z1     1|
| x2      y2     z2     1|
| x3      y3     z3     1| = 0
| x3+Mx   y3+My  z3+Mz  1|


so... uh... now you need to do the math for the cross and dot products to calculate the determinant.

... this might help Determinant Expansion by Minors

My head hurts now... and I'm not sure if all that is doable :-)

markcw

(Posted 2005) [#3]

would i sound really stupid if i said:

can you not just put an entity at the end
of one pivot, return its xyz position,
thus getting P4?

Jeppe Nielsen

(Posted 2005) [#4]

This does what you need, I think:

Just use the PoleVector() function:



Global camera,camerarx#,camerary#

Global polex#,poley#,polez#


Graphics3D 800,600,16,2

CameraInit()

plane=CreatePlane()
EntityAlpha plane,0.5
EntityPickMode plane,2
TurnEntity CreateLight(),45,45,0



;create our three points
p1=CreateSphere()
ScaleMesh p1,0.1,0.1,0.1
PositionEntity p1,0,0,0

p2=CreateSphere()
ScaleMesh p2,0.1,0.1,0.1
PositionEntity p2,0,2,4

p3=CreateSphere()
ScaleMesh p3,0.1,0.1,0.1
PositionEntity p3,3,0,0

;align plane to the 3 points
p1x#=EntityX(p1,1)
p1y#=EntityY(p1,1)
p1z#=EntityZ(p1,1)

p2x#=EntityX(p2,1)
p2y#=EntityY(p2,1)
p2z#=EntityZ(p2,1)

p3x#=EntityX(p3,1)
p3y#=EntityY(p3,1)
p3z#=EntityZ(p3,1)

dx12#=p2x-p1x
dy12#=p2y-p1y
dz12#=p2z-p1z

dx13#=p3x-p1x
dy13#=p3y-p1y
dz13#=p3z-p1z

AlignToVector plane,dx12,dy12,dz12,3
AlignToVector plane,dx13,dy13,dz13,1


PoleVector(p1,p2,p3,2,3)

p4=CreateSphere()
ScaleMesh p4,0.1,0.1,0.1
EntityColor p4,0,255,0

PositionEntity p4,polex,poley,polez

cyl1=CreateCylinder()
RotateMesh cyl1,90,0,0
PositionMesh cyl1,0,0,1
PositionEntity cyl1,EntityX(p1),EntityY(p1),EntityZ(p1)
ScaleMesh cyl1,0.05,0.05,EntityDistance(p1,p4)/2
PointEntity cyl1,p4
EntityColor cyl1,255,255,0

cyl2=CreateCylinder()
RotateMesh cyl2,90,0,0
PositionMesh cyl2,0,0,1
PositionEntity cyl2,EntityX(p3),EntityY(p3),EntityZ(p3)
ScaleMesh cyl2,0.05,0.05,EntityDistance(p3,p4)/2
PointEntity cyl2,p4
EntityColor cyl2,255,0,255





Repeat
Cls

If MouseDown(1)
	
	CameraPick(camera,MouseX(),MouseY())
	
	PositionEntity p3,PickedX(),PickedY(),PickedZ()
	
	PoleVector(p1,p2,p3,2,3)
	
	FreeEntity cyl1
	FreeEntity cyl2
	
	PositionEntity p4,polex,poley,polez
	
	cyl1=CreateCylinder()
	RotateMesh cyl1,90,0,0
	PositionMesh cyl1,0,0,1
	PositionEntity cyl1,EntityX(p1),EntityY(p1),EntityZ(p1)
	ScaleMesh cyl1,0.05,0.05,EntityDistance(p1,p4)/2
	PointEntity cyl1,p4
	EntityColor cyl1,255,255,0
	
	cyl2=CreateCylinder()
	RotateMesh cyl2,90,0,0
	PositionMesh cyl2,0,0,1
	PositionEntity cyl2,EntityX(p3),EntityY(p3),EntityZ(p3)
	ScaleMesh cyl2,0.05,0.05,EntityDistance(p3,p4)/2
	PointEntity cyl2,p4
	EntityColor cyl2,255,0,255
	
EndIf



If MouseDown(2)

cameraupdate 0.1

EndIf

RenderWorld

Text 400,10,"Left mouse button to place 3rd point",1
Text 400,25,"Right mouse button, mouse and arrow keys to move camera",1


Flip



Until KeyDown(1)
End

Function PoleVector(p1,p2,p3,r1#,r2#)

p1x#=EntityX(p1,1)
p1y#=EntityY(p1,1)
p1z#=EntityZ(p1,1)

p2x#=EntityX(p2,1)
p2y#=EntityY(p2,1)
p2z#=EntityZ(p2,1)

p3x#=EntityX(p3,1)
p3y#=EntityY(p3,1)
p3z#=EntityZ(p3,1)

dx12#=p1x-p2x
dy12#=p1y-p2y
dz12#=p1z-p2z

dx13#=p1x-p3x
dy13#=p1y-p3y
dz13#=p1z-p3z

;align p1 to the plane formed by the 3 points
AlignToVector p1,dx12,dy12,dz12,3
AlignToVector p1,dx13,dy13,dz13,1

;transform point 3 into p1's space
TFormPoint p3x,p3y,p3z,0,p1

;get the distance between p1 and p3 in p1's space
d#=TFormedX()

;equations taken from:
;http://mathworld.wolfram.com/Circle-CircleIntersection.html
x#=(d*d-r2*r2+r1*r1)/(2*d)
z#=-Sqr((4*d*d*r1*r1-(d*d-r2*r2+r1*r1)^2)/(4*d*d))

;transform points in p1's space to global space
TFormPoint x,0,z,p1,0

polex#=TFormedX()
poley#=TFormedY()
polez#=TFormedZ()

End Function

Function CameraInit(r=0,g=0,b=0,x#=10,y#=10,z#=-10,px#=0,py#=0,pz#=0)

	camera=CreateCamera()
	CameraClsColor camera,r,g,b
	PositionEntity camera,x,y,z
	p=CreatePivot()
	PositionEntity p,px,py,pz
	PointEntity camera,p
	FreeEntity p
	MoveMouse GraphicsWidth()/2,GraphicsHeight()/2
	camerarx=EntityPitch(camera)
	camerary=EntityYaw(camera)
		
End Function

Function CameraClear()

	If camera<>0 Then FreeEntity camera	: camera=0
	
End Function

Function CameraUpdate(speed#=1)

	camerarx#=camerarx#+MouseYSpeed()*0.5
	camerary#=camerary#-MouseXSpeed()*0.5
	If camerarx#>89
		camerarx#=89
	ElseIf camerarx#<-89
		camerarx#=-89
	EndIf
	RotateEntity camera,camerarx,camerary,0
	MoveMouse GraphicsWidth()/2,GraphicsHeight()/2
	
	If KeyDown(200) Or KeyDown(17) Then MoveEntity camera,0,0,speed#
	If KeyDown(208) Or KeyDown(31) Then MoveEntity camera,0,0,-speed#
	If KeyDown(203) Or KeyDown(30) Then MoveEntity camera,-speed#,0,0
	If KeyDown(205) Or KeyDown(32) Then MoveEntity camera,speed#,0,0
	
End Function

Danny

(Posted 2005) [#5]

Thanks guys!
I'll need some time to digest all this. I'm pretty close to solving it, and hoping I can post a working character IK solution soon...

And cheers for that Code Jeppe! You even got the colors right ;)

l8r-
Danny