Алгоритм
начало
1) i = номер посл. кости
2) вычисляем углы, на к-е нужно повернуть i-кость, чтобы эффектор совместился с вектором от корня i-кости до цели
3) поворачиваем i-кость на эти углы с применением прямой кинематики
4) i = i - 1
Если эффектор не достиг цели, переходим к началу, пока не кончится допустимое число итераций.
Код
Синтаксис:
Используется csharp
static Quaternion r;
static float dist;
static uint maxIterations = 30;
static public bool Solve1(KinematicLimb limb, Vector3 target)
{
if (target.magnitude > limb.Maxlen) return false;
uint iter = maxIterations;
do {
--iter;
uint i = limb.Num-1;
do {
r = Quaternion.FromToRotation(limb.Effector - limb.bones[ i ].Pos, target - limb.bones[ i ].Pos);
limb.bones[ i ].Rotate(r);
} while(i-- != 0);
dist = Mathf.Abs((limb.Effector - target).magnitude);
} while((dist > imprecision) && (iter != 0));
if (iter == 0) return false;
Debug.Log("iter = "+iter+", dist = "+dist);
return true;
}
static float dist;
static uint maxIterations = 30;
static public bool Solve1(KinematicLimb limb, Vector3 target)
{
if (target.magnitude > limb.Maxlen) return false;
uint iter = maxIterations;
do {
--iter;
uint i = limb.Num-1;
do {
r = Quaternion.FromToRotation(limb.Effector - limb.bones[ i ].Pos, target - limb.bones[ i ].Pos);
limb.bones[ i ].Rotate(r);
} while(i-- != 0);
dist = Mathf.Abs((limb.Effector - target).magnitude);
} while((dist > imprecision) && (iter != 0));
if (iter == 0) return false;
Debug.Log("iter = "+iter+", dist = "+dist);
return true;
}
Описание переменных:
limb - это объект "кинематическая конечность", к-я хранит положения костей и их ориентации относительно начала конечности (т. е. точка отсчета - пивот корневой кости).
target - координаты точки, куда нужно переместить эффектор (кончик последней кости), тоже относительно корневой кости.
limb:
Num - число костей в конечности
Maxlen - сумма длин всех костей (длина конечности)
iter - номер след. попытки расчета.
Effector - возвращает координаты эффектора конечности в родной системе координат
limb.bones[ i ].Rotate(r) - поворачивает i-кость на r-кватернион (а вместе с ней, все дочерние кости, т. к. в Unity есть прямая кинематика).
imprecision - минимальное расстояние до цели, при к-м цель считается достигнутой
Результат
Рука вращается как угорелая если цель движется. Если координаты цели не меняются, то вращается по постоянной траектории.
По отладочной информации видно что чаще всего возвращается false из-за истечения количества итераций.
Все выходные бился над этим кодом, не могу понять - в чем ошибка.
Если требуется, выложу весь код, занимающийся рукой.