forum.boolean.name
Страница 2 из 2 < 1 2
Показывать 64 сообщений этой темы на одной странице

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)

H@NON 28.01.2012 08:10
Ответ: Обратная кинематика. Правильные формулы?
 
вот, нашел на blitzmax.ru
Код:
;====================================================================
; Project: Invers Kinematics for rag doll
; Version: 0.01c
; Author: H@non
; Email: [email protected]
; Copyright: © H@non, 2010 г.
; Description:    http://www.gamedev.ru/articles/?id=30035
;                книга Рика Пэрента
; http://number-none.com/product/IK%20with%20Quaternion%20Joint%20Limits/index.html               
;====================================================================
 
 
Graphics3D 800, 600, 32, 2
SetBuffer BackBuffer()
 
 
Type StepTank
        Field korpus
        Field bone1[5], bone2[5]
        Field targetX#[5], targetY#[5], targetZ#[5]
End Type
Global ST.StepTank
 
Global Camera=CreateCamera ()
MoveEntity Camera, 0, 0, -10
 
Local lit = CreateLight()
RotateEntity lit, 45, 45, 0
 
SetFont LoadFont("arial cyr", 20, True)
 
Const GameUPS=60 ; Updates per second
Local Period=1000/GameUPS
Local FrameTime=MilliSecs()-Period
 
Local Tween#, Ticks,i,Remaining,StartTime,Elapsed
 
Global bone = CreateCube()
PositionMesh bone, 0, 0, 1
ScaleMesh bone, 0.4, 0.4, 3
HideEntity bone
 
Global target = CreateCube()
EntityColor target, 255, 0, 0
 
AddStepTank(0, 0, 0)
 
While Not KeyHit(1)
       
        StartTime = MilliSecs()
       
        Repeat
                Elapsed=MilliSecs()-FrameTime
        Until Elapsed
       
        Ticks=Elapsed / Period
        Tween=Float(Elapsed Mod Period)/Float(Period)
       
        For i=1 To Ticks
                FrameTime=FrameTime+Period
                If i=Ticks Then
                        CaptureWorld
                End If
                UpdateStepTank()
                MoveCamera(Camera)
               
                TranslateEntity target, KeyDown(205)-KeyDown(203), 0, KeyDown(200)-KeyDown(208)
               
                UpdateWorld
        Next
        RenderWorld Tween
       
        Text 10, 10, "Control :"
        Text 10, 30, "Camera : wasd + mouse"
        Text 10, 50, "target(red cube) : arrows(стрелки)"
       
        Remaining = Period - (MilliSecs() - StartTime)
        If Remaining > 1 Then
                Delay (Remaining-1) ; Free some CPU time
        End If
       
        Flip 0
       
Wend
 
End
 
;=========================================================================
 
 
Function AddStepTank(x#, y#, z#)
        ST.StepTank = New StepTank
        ST\bone1[0] = CopyEntity(bone)
        PositionEntity ST\bone1[0], x, y, z
        ;ScaleEntity ST\bone1[0], 0.4, 0.4, 6
       
        ST\bone2[0] = CopyEntity(bone)
        PositionEntity ST\bone2[0], x, y, z+6
        ;ScaleEntity ST\bone2[0], 0.4, 0.4, 6
        EntityColor ST\bone2[0], 255, 255, 0
        EntityParent ST\bone2[0], ST\bone1[0]
       
        ST\targetX[0] = 0;5
        ST\targetY[0] = 8
        ST\targetZ[0] = 5
       
        PositionEntity target, ST\targetX[0], ST\targetY[0], ST\targetZ[0]
End Function
 
Function UpdateStepTank()
        Local dx#, dy#, dz#
        Local torque#, diff#
        Local pitch#, yaw#, roll#
       
        TFormVector 0, 0, 6, ST\bone2[0], 0
        torque = CalculateAngleJoint(ST\bone1[0], TFormedX(), TFormedY(), TFormedZ(), target, 0.2)
        torque = limit(torque, -2, 2)
        TurnEntity ST\bone1[0], torque, 0, 0
       
       
        TFormVector 0, 0, 6, ST\bone2[0], 0
        torque = CalculateAngleJoint(ST\bone1[0], TFormedX(), TFormedY(), TFormedZ(), target, 0.2, 2)
        torque = limit(torque, -2, 2)
        TurnEntity ST\bone1[0], 0, torque, 0;, False
        ;TurnEntity ST\bone1[0], 0, 0, -EntityRoll(ST\bone1[0],1)
        ;RotateEntity ST\bone1[0], EntityPitch(ST\bone1[0],1), EntityYaw(ST\bone1[0],1), 0, True
       
       
        TFormVector 0, 0, 6, ST\bone1[0], 0
        torque = CalculateAngleJoint(ST\bone2[0], TFormedX(), TFormedY(), TFormedZ(), target, 0.2)
        torque = limit(torque, -2, 2)
        TurnEntity ST\bone2[0], torque, 0, 0
       
        TFormVector 0, 0, 6, ST\bone1[0], 0
        torque = CalculateAngleJoint(ST\bone2[0], TFormedX(), TFormedY(), TFormedZ(), target, 0.2, 2)
        torque = limit(torque, -2, 2)
        TurnEntity ST\bone2[0], 0, torque, 0;, True
End Function
 
Function CalculateAngleJoint#(joint, boneX#, boneY#, boneZ#, target, Sens#=1.0, Axis=1)
        Local Rx#, Ry#, Rz#
        Local Fx#, Fy#, Fz#
        Local Ax#, Ay#, Az#
        Local mag#
       
        Fx# = EntityX(target,1) - boneX
        Fy# = EntityY(target,1) - boneY
        Fz# = EntityZ(target,1) - boneZ
       
        mag# = Sqr(Fx*Fx + Fy*Fy + Fz*Fz)
       
        Select Axis
                ;-------------------------------------------------------
                Case 1
                ;- вектор вдоль оси шарнира
                TFormVector 1, 0, 0, joint, 0
                Ax# = TFormedX()
                Ay# = TFormedY()
                Az# = TFormedZ()
                Rx = Sin(VectorAngle#(Ax#,Ay#,Az#, Fx#,Fy#,Fz#))
                ;- вектор вдоль кости
                TFormVector 0, 0, 1, joint, 0
                Ax# = TFormedX()
                Ay# = TFormedY()
                Az# = TFormedZ()
                Ry = Sin(VectorAngle#(Ax#,Ay#,Az#, Fx#,Fy#,Fz#))
                ;- вектор перпендикулярный векторам оси шарнира и кости
                TFormVector 0, 1, 0, joint, 0
                Ax# = TFormedX()
                Ay# = TFormedY()
                Az# = TFormedZ()
                Rz = -Cos(VectorAngle#(Ax#,Ay#,Az#, Fx#,Fy#,Fz#))
                ;-------------------------------------------------------
                Case 2
                TFormVector 0, 1, 0, joint, 0
                Ax# = TFormedX()
                Ay# = TFormedY()
                Az# = TFormedZ()
                Rx = Sin(VectorAngle#(Ax#,Ay#,Az#, Fx#,Fy#,Fz#))
                TFormVector 0, 0, 1, joint, 0
                Ax# = TFormedX()
                Ay# = TFormedY()
                Az# = TFormedZ()
                Ry = Sin(VectorAngle#(Ax#,Ay#,Az#, Fx#,Fy#,Fz#))
                TFormVector 1, 0, 0, joint, 0
                Ax# = TFormedX()
                Ay# = TFormedY()
                Az# = TFormedZ()
                Rz = -Cos(VectorAngle#(Ax#,Ay#,Az#, Fx#,Fy#,Fz#))
                ;-------------------------------------------------------
                Case 3
                TFormVector 0, 0, 1, joint, 0
                Ax# = TFormedX()
                Ay# = TFormedY()
                Az# = TFormedZ()
                Rx = Sin(VectorAngle#(Ax#,Ay#,Az#, Fx#,Fy#,Fz#))
                TFormVector 1, 0, 0, joint, 0
                Ax# = TFormedX()
                Ay# = TFormedY()
                Az# = TFormedZ()
                Ry = Sin(VectorAngle#(Ax#,Ay#,Az#, Fx#,Fy#,Fz#))
                TFormVector 0, 1, 0, joint, 0
                Ax# = TFormedX()
                Ay# = TFormedY()
                Az# = TFormedZ()
                Rz = -Cos(VectorAngle#(Ax#,Ay#,Az#, Fx#,Fy#,Fz#))
                ;------------------------------------------
        End Select
       
        Return (mag*Rx*Ry*Sgn(Rz))*Sens
End Function
 
Function MoveCamera(camera, speed#=0.5)
        If MouseDown(1) Then
                Local mxs# = MouseXSpeed()
                Local mys# = MouseYSpeed()
                If MouseHit(1) Then
                        mxs = 0
                        mys = 0
                EndIf
                RotateEntity camera, EntityPitch(camera)+mys*0.5, EntityYaw(camera)-mxs*0.5, 0
                MoveMouse 400, 300
        EndIf
       
        Local up = KeyDown(17)
        Local down = KeyDown(31)
        Local Left = KeyDown(30)
        Local Right = KeyDown(32)
       
        MoveEntity camera, (Right-Left)*speed, 0, (up-down)*speed
End Function
 
 
Function RotEntity(ent, pit#=0, yaw#=0, roll#=0)
        RotateEntity ent, EntityPitch(ent,1)+pit, EntityYaw(ent,1)+yaw, EntityRoll(ent,1)+roll, True
End Function
 
Function VectorAngle#(Ax#,Ay#,Az#, Bx#,By#,Bz#)
        Local d# = VectorDot(Ax#,Ay#,Az#, Bx#,By#,Bz#)
        Local m# = VectorMagnitude(Ax#,Ay#,Az#)*VectorMagnitude(Bx#,By#,Bz#)
        Return ACos(d#/m#)
End Function
 
Function VectorDot#(Ax#,Ay#,Az#, Bx#,By#,Bz#)
        Return (Ax*Bx) + (Ay*By) + (Az*Bz)
End Function
       
Function VectorMagnitude#(Ax#,Ay#,Az#)
        Return Sqr(Ax*Ax + Ay*Ay + Az*Az)
End Function
 
Function AngleDiff#(angle1#,angle2#)
        Return ((angle2 - angle1) Mod 360 + 540) Mod 360 - 180
End Function
 
Function limit#(val#, min#, max#)
        If val < min Then
                Return min
        ElseIf val > max Then
                Return max
        Else
                Return val
        EndIf
End Function
 
Function DELTAroll#( Source , Target )
        TFormPoint 0,0,0 , Target, Source
        Return VectorYaw ( TFormedX() , 0 , TFormedY() )
End Function

CRASHER 29.01.2012 21:09
Ответ: Обратная кинематика. Правильные формулы?
 
Мой код конечно не настолько мобилен. но вот он кажется попроще))

Код:
Function CircleIntersec(X1#,Y1#,R1#,X2#,Y2#,R2#)
        vx#=X2-X1;
        vy#=Y2-Y1;
        vv#=vx*vx+vy*vy;
        If ((R1+R2)*(R1+R2)<vv)
                Return False;
        Else
                a#=(R1+R2)*(R1-R2)/2/vv+0.5;
                b#=Sqr(R1*R1/vv-a*a);
                P1[0]=X1+a*vx-b*vy;
                P1[1]=Y1+a*vy+b*vx;
                P2[0]=X1+a*vx+b*vy;
                P2[1]=Y1+a*vy-b*vx;
                Return True;
        EndIf
End Function

В цикле

XHand#=xEntityX(point3)-xEntityX(Cup1_1)
YHand#=xEntityY(point3)-xEntityY(Cup1_1)

B#=Sqr(Ext(XHand)+Ext(YHand))

qq1#=ATan2#(YHand#,XHand#)
qq2#=(ACos#((Ext#(L1)-Ext#(L2)+Ext#(B))/(2*L1*B)))

If B>(L1+L2)
        Q1#=0
        Q2#=15
        xPointEntity(Cup1_1,point3)
        xRotateEntity(Cup1_1, 0, 0, -xEntityPitch(Cup1_1)+90)
Else
        Q1#=qq1+qq2+90
        Q2#=ACos#((Ext#(L1)+Ext#(L2)-Ext#(B))/(2*L1*L2))+180
        xRotateEntity(Cup1_1, 0, 0, Q1)
        xRotateEntity(Cup1_2, 0, 0, Q2)
EndIf
Я развиваю пока идею с точкой, чтобы можно было вертетьв трех плоскостях.

DarkInside 30.01.2012 17:48
Ответ: Обратная кинематика. Правильные формулы?
 
Цитата:
Сообщение от CRASHER (Сообщение 217508)
Mr_F_,dsd, да знаю я что такое sqrt и sqr. В блице корень sqr, а квадрата нет, вот я и сделал чтоб не мучится
оу:4to: а это что по вашему x^2? Вот это "внатуре алоэ" - писать функцию для возведения в квадрат:-D Простите, не удержался...

По теме, к сожалению, ничего сказать не могу, у меня всегда было плохо с физикой...

Вот еще парочка полезных функций:super:
Код:
Function plus#(a, b)
c=a+b
Return c
End Function

Function minus#(a, b)
c=a-b
Return c
End Function

CRASHER 30.01.2012 18:04
Ответ: Обратная кинематика. Правильные формулы?
 
DarkInside, не знал об этом ну и что?

CRASHER 30.01.2012 18:08
Ответ: Обратная кинематика. Правильные формулы?
 
DarkInside, тебе занятся нечем? По теме ничего сказать не можешь? Проверять чужой код на синтаксис решил?

DarkInside 30.01.2012 18:08
Ответ: Обратная кинематика. Правильные формулы?
 
теперь знай;) Ну зачем так грубо сразу))

CRASHER 30.01.2012 18:11
Ответ: Обратная кинематика. Правильные формулы?
 
DarkInside, потому что это не красиво! Одно дело когда ошибка ведт к не рабочему коду, и другое дело когда тебе хочется сконцентрировать внимание на интерпритации.


Часовой пояс GMT +4, время: 04:55.
Страница 2 из 2 < 1 2
Показывать 64 сообщений этой темы на одной странице

vBulletin® Version 3.6.5.
Copyright ©2000 - 2025, Jelsoft Enterprises Ltd.
Перевод: zCarot