достал формулы и попытался закодировать.
Не работают как надо, я их вроде не менял, значит я что то не понял или формулы под сомнением ?

Include "Xors3d.bb"
xGraphics3D 800, 600, 32, 0, 1
arial = xLoadFont("Arial", 12);Arial
pxCreateWorld(0, "клуч")
pxSetGravity(0, -10, 0)
Global camera,mys#,mzs#,mxs#,znam#=5,MouseMove=1,lx#,ly#,lz#,Getx#=20,Gety#=20
Global mzP#,cameraPiv
Const WiHi#=2
cameraPiv=xCreatePivot()
camera = xCreateCamera()
xPositionEntity camera, 0, 10, -15
xEntityParent(camera,cameraPiv)
Textura=xLoadTexture ("Textures\terrain2.jpg")
light_1=xCreateLight (1)
xPositionEntity(light_1,10,5,10)
plan=xCreateCube()
xScaleEntity(plan,1000,0.1,1000)
xEntityTexture(plan,Textura)
xScaleTexture(Textura,0.01,0.01)
xEntityPickMode plan,2
Cup_1=xCreateCube()
xPositionEntity Cup_1, 0, 8, 0
xScaleMesh(Cup_1,0.5,0.5,0.5)
Cup1_1=xCreateCube()
xPositionEntity Cup1_1, 0, 8, 0
xPositionMesh Cup1_1, 0, -1.5, 0
xScaleMesh(Cup1_1,0.5,1.5,0.5)
Cup1_2=xCreateCube(Cup1_1)
xPositionEntity Cup1_2, 0, -3.5, 0
xPositionMesh Cup1_2, 0, -1, 0
xScaleEntity(Cup1_2,0.5,1.5,0.5)
xWorldSetGravity (0,-20,0)
Global Rot2#,Rot#
point3=xCreateSphere()
xScaleEntity(point3,0.5,0.5,0.5)
xPositionEntity(point3,1, 0, 0)
CM=xCreateSphere()
Function sqrt#(x)
x=x*x
Return x
End Function
Function RadToGrad#(x)
x=(x*180)/Pi
Return x
End Function
;_____________________________________________________________________________________________________
While Not xKeyDown(1)
L1=3
L2=3
XHand#=xEntityX(Cup1_1)-xEntityX(point3)
YHand#=xEntityY(Cup1_1)-xEntityY(point3)
B#=Sqr((sqrt#(XHand)+sqrt#(YHand)));xEntityDistance(Cup_1,point3)
q1#=ATan2#(XHand#,YHand#)
q2#=(ACos#((sqrt#(L1)-sqrt#(L2)+sqrt#(B))/(2*L1*B)))
;If Sqr((sqrt#(XHand)+sqrt#(YHand)))<B
Q1#=q1+q2
Q2#=ACos#((sqrt#(L1)+sqrt#(L2)-sqrt#(B))/(2*L1*L2))
; Q1#=RadToGrad(Q1)
; Q2#=RadToGrad(Q2)
;Else
; Q1=90
; Q2=90
;EndIf
xRotateEntity(Cup1_1, 0, 0, Q1)
xRotateEntity(Cup1_2, 0, 0, Q2)
DebugLog("Dist(1-3)= "+B)
DebugLog("Q1= "+Q1)
DebugLog("Q2= "+Q2)
If xKeyDown (57) xEntityApplyTorque(Cup1_1,-20,30,-5):xEntityApplyForce(Cup1_1,-5,6,-5,0,0,0)
mys=xMouseYSpeed()
mxs=xMouseXSpeed()
mzs=xMouseZSpeed()
If xKeyDown(32)=True Then xMoveEntity point3,0.2,0,0
If xKeyDown(30)=True Then xMoveEntity point3,-0.2,0,0
If xKeyDown(31)=True Then xMoveEntity point3,0,-0.2,0
If xKeyDown(17)=True Then xMoveEntity point3,0,0.2,0
If xKeyDown(205)=True Then xMoveEntity camera,0.4,0,0
If xKeyDown(203)=True Then xMoveEntity camera,-0.4,0,0
If xKeyDown(208)=True Then xMoveEntity camera,0,0,-0.4
If xKeyDown(200)=True Then xMoveEntity camera,0,0,0.4
Update_user(camera,mxs,mys)
xUpdateWorld ()
xRenderWorld()
Text 10,10, "text="
xFlip()
Wend
;----------------------------
End
Function Update_user(camera,mxs#,mys#)
If mys>30 Then mys=30
If mys<-30 Then mys=-30
If mxs>30 Then mxs=30
If mxs<-30 Then mxs=-30
If MouseMove=1
xTurnEntity camera,mys/znam,0,0
xTurnEntity camera,0,-mxs/znam,0,1
EndIf
u#=80
xMoveMouse xGraphicsWidth()*0.5,xGraphicsHeight()*0.5
;xHidePointer
If Abs(xEntityPitch#(camera))>u# xRotateEntity camera,(u#)*Sgn(xEntityPitch#(camera)),xEntityYaw(camera),0
End Function