[H@NON]

вот, нашел на 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