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

[CRASHER]

значить с сайта 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

[dsd]

Function sqrt#(x)
x=x*x
Return x
End Function

вообще судя по названию там корень должен считаться, а у тебя возводится в квадрат. Так что вполне может быть, что все косяки из-за этой ф-ции.

[Mr_F_]

в блице есть же корень, только называется sqr.
если не втыкаешь почему формулы именно такие как написаны, то ничего не выйдет с тупого их списывания. надо теоретически понимать алгоритм.
мне кажется сайт неподходящий. там нифига непонятно что написано. ищи инфу для разработчиков игр, а не роботостроителей.
--
вот самая понятная дока, которую я нашёл: http://www.darwin3d.com/gamedev/articles/col0998.pdf

[CRASHER]

Mr_F_,dsd, да знаю я что такое sqrt и sqr. В блице корень sqr, а квадрата нет, вот я и сделал чтоб не мучится, ну и назвал так. Кажется где то видел что в си принято такое обозначение. Если ошибся, ну так тогда как обозначить? Одно ясно под sqrt я подразумеваю квадрат и в формулах где я его использую тоже квадрат. Здесь что то в этих формулах не то кажется.

[CRASHER]

вот самая понятная дока, которую я нашёл: http://www.darwin3d.com/gamedev/articles/col0998.pdf

Mr_F_, а ты уверен что там есть что мне нужно?

[dsd]

А что не так с кинематикой? Есть система стержней соединенных шарнирами. Есть её текущее положение. Есть точка куда должен уткнуться конец системы.

Конец последнего стержня может перемещаться по поверхности сферы с центром на шарнире и радиусом в длину стержня. Если координаты цели есть решение уравнения сферы, значит последний элемент можно повернуть относительно его шарнира связующего его с родителем и цель достигнута.

Если нет. Смотрим на пересечение сферы родителя со сферой радиусом в длину последнего стержня вокруг точки цели. Если есть пересечение то можно поставить в ближайшую точку от текущего положения родителя и уткнуть последний стержень в цель.

Если пересечений нет, то проверяем сферу досягаемости родителя уровнем выше и сферу вокруг цели радиусом в сумму длины последнего и предпоследнего стержня. И т.д.

Если предки кончились, а точка так и не достигнута, то тупо прицеливаем усе стержни в цель. Как то так

[CRASHER]

dsd, я попытался понять. Не буду врать, не уверен что всё что ты сказал мне понятно.) Ты сейчас говоришь о статье которую Mr_F предложил?

[dsd]

Нет, я говорю о термехе и геометрии в пространстве.

[CRASHER]

dsd, возможно в закодированном виде было бы понятнее))

[dsd]

[|xqѪTѤzѤyqѤt~ѤѤ~|yqxt|~z}ѻѤQѤ|qxtѤqryq}Ѥzqp|yqyyѤt y|tx|ѪѤQѤqѤq~
qqѤ{zzrqy|qѪѤQѤz~tѤ~ptѤpzrqyѤ~yѤ~zyqѤ|qxѪщщ^zyqѤ {zqpyqwzѤqryѤxzrqѤ{qqxq
tѤ{zѤ{zvqyz|ѤqѤѤqyzxѤytѤty|qѤ|Ѥtp|zxѤvѤp|yѤqryѪѤQ |Ѥ~zzp|ytѤq|ѤqѤqqy|qѤtvyqy|ѤqѨѤsyt|Ѥ{zqpy|}Ѥ qxqyѤxzryzѤ{zvqyѤzyz|qyzѤqwzѤty|tѤvs

qwzѤqwzѤѤzp|qqxѤ|ѤqѤpz|wytѪщщQ|ѤyqѪѤexz|xѤytѤ{q qqqy|qѤqѤzp|qѤzѤqz}Ѥtp|zxѤvѤp|yѤ{zqpyqwzѤqryѤvz ~wѤz~|Ѥq|ѪѤQ|ѤqѤ{qqqqy|qѤzѤxzryzѤ{ztv|ѤvѤu|r t}
Ѥz~ѤzѤq~
qwzѤ{zzrqy|Ѥzp|qѤ|Ѥ~yѤ{zqpy|}ѤqrqyѤvѤqѪщщQ|Ѥ{ qqqqy|}ѤyqѨѤzѤ{zvqqxѤqѤpzwtqxz|Ѥzp|qѤzvyqxѤvqѤ|Ѥq Ѥvz~wѤq|Ѥtp|zxѤvѤxxѤp|yѤ{zqpyqwzѤ|Ѥ{qp{zqpyqwz ѤqryѪѤ\ѤѪpѪщщQ|Ѥ{qp~|Ѥ~zy||ѨѤtѤz~tѤt~Ѥ|ѤyqѤpz|wy tѨѤzѤ{zѤ{|q|vtqxѤqѤqry|ѤvѤqѪѤ^t~ѤzѤt~

символ шифровки: ф

http://codedecode.ru/

Вааще там самое сложное это найти нужную точку пересечения двух сфер.
Када сферы пересекаюца или касаюца? Когда расстояние между центрами меньше равно суммы радиусов. Какова нормаль плоскости по которой пересекаются две сферы? Совпадает с линией проходящей через их центры.
Где точка на линии соединяющей центры сфер принадлежащая плоскости сечения?
Отмеряем от центра первой радиус второй. Находим разницу между радиусами. Прибавляем ко второму радиусу разницу умноженную на соотношение радиусов.
Каков радиус окружности на этой плоскости? Расстояние от центра до точки сечения это косинус угла на радиус. => синус на радиус = радиус окружности на плоскости. Дальше ищем пересечение прямой по которой стержень лежит с плоскостью сечения. Получается точка.
Считаем вектор от этой точки до центра окружности. делим его на его длину и умножаем на его длину минус радиус окружности. Получилась нужная точка куда воткнуть текущий стержень. Считаем вектор от шарнира до этой точки. xAlignEntityToVector(). Про точку места сечения может и наврал

[CRASHER]

dsd, псиб терь все понятно. Да еще, закодированный вид я подразумевал код языка программирования, если ты не понял.

[CRASHER]

Вообщем, глупые формулы! Нашел рабочие и реализовал. а эти в топку.

[H@NON]

код в студию)

[CRASHER]

Иш ты налетели)) Как помоч так нету, а как код так они тут уже))
Лан, вам какой с вычислением точки или с углами?

[H@NON]

у меня был пример вычисления на углах, я еще на блитц.пп выкладывал, но после полета диска демка исчезла.

Давай с вычислением углов.