forum.boolean.name - Обратная кинематика. Правильные формулы?

forum.boolean.name (http://forum.boolean.name/index.php)

- 3D-программирование (http://forum.boolean.name/forumdisplay.php?f=12)

- - Обратная кинематика. Правильные формулы? (http://forum.boolean.name/showthread.php?t=16221)

Обратная кинематика. Правильные формулы?

значить с сайта http://roboty6.narod.ru/inverseKinematics.htm
достал формулы и попытался закодировать.

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

Вот!

Код:

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