Показать сообщение отдельно
Старый 22.01.2012, 10:58   #1
CRASHER
Разработчик
 
Регистрация: 08.03.2007
Сообщений: 530
Написано 31 полезных сообщений
(для 36 пользователей)
Обратная кинематика. Правильные формулы?

значить с сайта 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
(Offline)
 
Ответить с цитированием