Math Help needed: Polevector for Ik System

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