三角函数求解
我们在2D逆向运动学：三角函数求解中介绍了如何在2D空间中实现双关节带末端效应器的机械臂，本篇我们会全面分析如何在3D空间中应用该方法，并另外介绍理论上可以不限关节数量的铰链方案，通过Fabrik(前向和反向迭代逆运动学)，该方法也可以很形象的想象为它是一种拉锯算法。实现的效果如下：
通过三角函数求解的3D IK：
通过Fabrik实现的多关节铰链:
通过3D IK实现蜘蛛行走的效果:
三角函数求解3D IK
在2D逆向运动学中已经介绍2D机械臂如何实现，及实现的求取要点是要分别求取q1与q2的旋转角度，如下图所示：
转换到3D空间中，我们除了要求取q1与q2的角度之外，还需要求取首个关节在Y轴上的旋转角度。如下图：
3D空间中机械臂有A与B两个关节，其中E为末端效应器。C点为E点在xz平面上的投影，并且AC与CE互相垂直。所以最终要求取的是如下图所示的q1，q2，q3的旋转角度：
其中q3是关节A绕Y轴的旋转角度，∠CAB=q1，且q1=O1+O2的角度之和，q2=PI - ∠EBA。分解之后，我们需要求取的即是，q3，O1，O2与∠EBA的角度。
为了求取上面所述的角度，我们首先需要求取AC向量，以求取AD，DC，AC及CE的长度。因为：
$$
AD = E_x-A_x
$$
$$
DC = E_z-A_z
$$
$$
CE = E_y-A_y
$$
又因为:
$$
AC=\sqrt{AD^2+DC^2}
$$
所以我们可以 首先得到q3：
$$
q_3 = arctan(DC, AD) = arctan(E_z-A_z, E_x-A_x)
$$
然后我们得到O1:
$$
O_1=arctan(CE, AC)=arctan(E_y-A_y, \sqrt{(E_x-A_x)^2+(E_z-A_z)^2})
$$
得到O2:
$$
O_2=arccos((|AB|^2+|AE|^2-|BE|^2)/(2|AB||AE|))
$$
所以 得到了q1：
$$
\begin{align}
q_1=arctan(E_y-A_y, \sqrt{(E_x-A_x)^2+(E_z-A_z)^2}) \
+arccos((|AB|^2+|AE|^2-|BE|^2)/(2|AB||AE|))
\end{align}
$$
得到q2：
$$
q_2=PI-arccos((|AB|^2+|BE|^2-|AE|^2)/(2|AB||BE|))
$$
这是实现3D三角函数IK的完整代码：
#define STEP 200.
#define R 0.1
float sdCylinder(vec4 p, float r, float h) {
return max(length(p.xz)-r, abs(p.y)-h);
}
float sdSphere(vec4 p, float r) {
return length(p.xyz)-r;
}
float sdBox(vec4 p, vec3 b) {
vec3 q = abs(p.xyz) - b;
return length(max(q,0.0)) + min(max(q.x,max(q.y,q.z)),0.0);
}
// 创建平移矩阵（沿XYZ轴移动）
mat4 translate(float tx, float ty, float tz) {
return mat4(
1, 0, 0, 0, // 第一列
0, 1, 0, 0, // 第二列
0, 0, 1, 0, // 第三列
tx, ty, tz, 1 // 第四列（平移分量）
);
}
// 向量参数版本
mat4 translate(vec3 t) {
return translate(t.x, t.y, t.z);
}
// 创建均匀缩放矩阵
mat4 scale(float s) {
return mat4(
s, 0, 0, 0, // 第一列
0, s, 0, 0, // 第二列
0, 0, s, 0, // 第三列
0, 0, 0, 1 // 第四列
);
}
// 非均匀缩放（各轴独立缩放）
mat4 scale(vec3 s) {
return mat4(
s.x, 0, 0, 0,
0, s.y, 0, 0,
0, 0, s.z, 0,
0, 0, 0, 1
);
}
mat4 rotateX(float theta) {
float c = cos(theta);
float s = sin(theta);
return mat4(
1, 0, 0, 0,
0, c, s, 0, // Y和Z轴的变化
0,-s, c, 0,
0, 0, 0, 1
);
}
mat4 rotateY(float theta) {
float c = cos(theta);
float s = sin(theta);
return mat4(
c, 0,-s, 0, // X和Z轴的变化
0, 1, 0, 0,
s, 0, c, 0,
0, 0, 0, 1
);
}
mat4 rotateZ(float theta) {
float c = cos(theta);
float s = sin(theta);
return mat4(
c, s, 0, 0, // X和Y轴的变化
-s, c, 0, 0,
0, 0, 1, 0,
0, 0, 0, 1
);
}
float smin( float a, float b, float k ) {
float h = clamp( 0.5+0.5*(b-a)/k, 0., 1. );
return mix( b, a, h ) - k*h*(1.0-h);
}
float smax(float a, float b, float k) {
return smin(a, b, -k);
}
float hash(float x) {
return fract(sin(x * 13452.) * 1000.);
}
float hash2(vec2 v) {
return fract(sin(dot(v, vec2(64.24232, 87.873)))*10232.);
}
float hash3(vec3 v) {
return fract(sin(dot(v, vec3(64.24232, 87.873, 76.635)))*10232.);
}
float sdCapsule( vec3 p, vec3 a, vec3 b, float r )
{
vec3 pa = p - a, ba = b - a;
float h = clamp( dot(pa,ba)/dot(ba,ba), 0.0, 1.0 );
return length( pa - ba*h ) - r;
}
float valueNoise(vec2 v) {
vec2 f = floor(v);
vec2 r = smoothstep(vec2(0.), vec2(1.), fract(v));
float lb = hash2(f);
float rb = hash2(f+vec2(1.,0.));
float lt = hash2(f+vec2(0.,1.));
float rt = hash2(f+vec2(1.,1.));
float b_line = mix(lb, rb, r.x);
float t_line = mix(lt, rt, r.x);
return mix(b_line, t_line, r.y);
}
float valueNoise3(vec3 v) {
vec3 f = floor(v);
vec3 r = smoothstep(vec3(0.), vec3(1.), fract(v));
return mix(
mix(mix(hash3(f), hash3(f+vec3(1.,0.,0.)), r.x),
mix(hash3(f + vec3(0.,1.,0.)), hash3(f + vec3(1.,1.,0.)), r.x),
r.y),
mix(mix(hash3(f+vec3(0.,0.,1.)), hash3(f+vec3(1.,0.,1.)), r.x),
mix(hash3(f + vec3(0.,1.,1.)), hash3(f + vec3(1.,1.,1.)), r.x),
r.y),
r.z
);
}
void calcIK(inout vec4 A, inout vec4 B, inout vec4 C,
inout vec4 jaw1, inout vec4 jaw2,vec4 target) {
vec4 lA = A, lB = B, lC = C;
mat4 combineMat;
mat4 wAM = translate(vec3(A.x, A.y, A.z));
vec4 wB = wAM * B;
mat4 wBM = wAM * translate(vec3(B.xyz));
vec4 wC = wBM * C;
float LA = length(wB.xyz - A.xyz);
float LB = length(wC.xyz - wB.xyz);
vec3 AT = target.xyz - A.xyz;
float LT = length(AT);
float diffTy = target.y - A.y;
float diffTx = target.x - A.x;
float diffTz = target.z - A.z;
float zRotAngle = atan(diffTy, sqrt(diffTx*diffTx + diffTz*diffTz));
float yRotAngle = atan(diffTz, diffTx);
if((LA+LB) < LT) {
wAM *= rotateY(-yRotAngle) * rotateZ(zRotAngle);
combineMat = wAM * translate(lB.xyz);
C = combineMat * lC;
float rotateVal = iTime*4.;
mat4 jawMat = combineMat * translate(lC.xyz) * rotateX(rotateVal);
jaw1 = jawMat * jaw1;
jaw2 = jawMat * jaw2;
B = wAM * lB;
} else {
float LA2 = LA*LA;
float LT2 = LT*LT;
float LB2 = LB*LB;
float z2RotAngle = acos((LA2 + LT2-LB2)/(2.*LA*LT));
float zBRotAngle = -(PI - acos((LA2 + LB2-LT2)/(2.*LA*LB)));
wAM *= rotateY(-yRotAngle) * rotateZ(zRotAngle+z2RotAngle);
combineMat = wAM * translate(lB.xyz) * rotateZ(zBRotAngle);
C = combineMat * lC;
float rotateVal = iTime*4.;
mat4 jawMat = combineMat * translate(lC.xyz) * rotateX(rotateVal);
jaw1 = jawMat * jaw1;
jaw2 = jawMat * jaw2;
B = wAM * lB;
}
}
vec2 dist(vec3 p) {
float mat = 0.;
vec4 p_4 = vec4(p,1.);
// joint worldPos
vec4 A = vec4(0., -.3, 0., 1.);
// joint localPos
vec4 B = vec4(1., 0., 0., 1.);
vec4 C = vec4(.8, 0., 0., 1.);
// jaw
vec4 jaw1 = vec4(0., 0., .1, 1.);
vec4 jaw2 = vec4(0.25, 0., .1, 1.);
// world pos
float yPos =mix(hash(floor(iTime)), hash(floor(iTime))+1., fract(iTime));
float h = 0.1;
h =mix(h, 0.8, smoothstep(2., 3., mod(iTime, 6.)) - smoothstep(3., 4., mod(iTime, 6.)));
vec4 tar = vec4(sin(iTime)*1.5, abs(sin(iTime*5.))*h, (cos(iTime))*0.6, 1.);
float d = 0.;
float target = sdSphere(vec4(p_4.xyz - tar.xyz, 1.), 0.1);
calcIK(A, B, C, jaw1, jaw2, tar);
float armT = sdCapsule(p, A.xyz, B.xyz, R);
float armB = sdCapsule(p, B.xyz, C.xyz, R);
d = min(target, min(armT, armB));
vec2 arm = vec2(d, 2.);
float t = iTime;
// 底座
d = sdCylinder(vec4(p - vec3(0,-0.7,0), 1.), 1.3, 0.01);
// 立柱
d = smin(d, sdCylinder(vec4(p - vec3(0,-0.8,0), 1.), 0.2, 0.4), 0.1);
// 第一关节
vec4 p1 = (vec4(p - A.xyz, 1.));
d = smin(d, sdSphere(p1, 0.15), 0.1);
// 大臂
d = smin(d, sdCapsule(p.xyz, A.xyz, B.xyz, 0.1), 0.1);
// 第二关节
vec4 p3 = vec4(p.xyz - B.xyz, 1.);
d = smin(d, sdSphere(p3, 0.12), 0.03);
// 小臂
d = smin(d, sdCapsule(p.xyz, B.xyz, C.xyz, 0.1), 0.03);
// 夹爪
vec3 dirCB = normalize(B.xyz-C.xyz);
d = smin(d, sdCapsule(p.xyz, C.xyz+dirCB*0.07, C.xyz, 0.14), 0.01);
d = smin(d, sdCapsule(p.xyz, jaw1.xyz, jaw2.xyz, 0.04), 0.03);
vec3 offset = normalize(C.xyz - jaw1.xyz)*0.1;
vec3 jaw1_ = C.xyz + offset;
vec3 jaw2_ = jaw2.xyz + 2. * offset;
d = smin(d, sdCapsule(p.xyz, jaw1_, jaw2_, 0.04), 0.03);
float targetSphere = sdSphere(vec4(p.xyz-jaw2_ + offset, 1.), 0.06);
if(targetSphere < d) {
mat = 1.;
}
d = min(d, targetSphere);
return vec2(d, mat);
}
vec3 calNormal(vec3 p) {
vec2 k = vec2(0.001, 0.);
return normalize(vec3(
dist(p + k.xyy).x - dist(p - k.xyy).x,
dist(p + k.yxy).x - dist(p - k.yxy).x,
dist(p + k.yyx).x - dist(p - k.yyx).x
));
}
vec2 raymarch(vec3 ro, vec3 rd) {
float t = 0.;
float y = -1.;
for(float i = 0.; i<STEP; i++) {
vec2 d = dist(ro + rd*t);
if(d.x < .001) {
y = d.y;
break;
}
if(t > STEP) {
t = -1.;
break;
}
t += d.x;
}
return vec2(t, y);
}
void mainImage( out vec4 fragColor, in vec2 fragCoord )
{
// Normalized pixel coordinates (from -1 to 1)
vec2 uv = 2. * (fragCoord - 0.5*iResolution.xy) / iResolution.y;
vec3 skyCol = vec3(0.302,0.298,0.298);
vec3 col = skyCol;
vec3 ro = vec3(0.,-.2,1.9);
vec3 rd = normalize(vec3(uv.xy, -1.));
vec2 t = raymarch(ro, rd);
if(t.x > 0.) {
vec3 mate = vec3(0.2);
vec3 sunCol = vec3(1.);
vec3 p = ro + rd*t.x;
vec3 n = calNormal(p);
if(t.y < .5) {
vec3 sunDir = normalize(vec3(0.8,0.4,0.2));
float sunDiff = max(0., dot(n, sunDir));
float skyDiff = clamp(0.5 + 0.5*dot(vec3(0.,1.,0.), n), 0., 1.);
float sunShadow = step(raymarch(p + n*0.01, sunDir).x, 0.);
col = mate * vec3(8.0,5.0,3.0) * sunDiff * sunShadow;
col += mate * skyDiff * vec3(0.2, 0.6, 1.);
} else if(t.y < 1.5) {
vec3 viewDir = normalize(ro - p);
col = pow(1.-dot(viewDir, n)+.8, 5.) * vec3(0.2, 0.6, 1.);
}
}
col = pow(col, vec3(0.4545));
// Output to screen
fragColor = vec4(col,1.0);
}
主要分析下calcIK函数，其中A为世界坐标：
// 计算逆向运动学（IK）的逻辑，用于调整机械臂的关节位置，使其接近目标位置
void calcIK(inout vec4 A, inout vec4 B, inout vec4 C,
inout vec4 jaw1, inout vec4 jaw2, vec4 target) {
vec4 lA = A, lB = B, lC = C; // 保存坐标的初始值，其中A为世界坐标，B与C为本地坐标
mat4 combineMat; // 用于存储组合变换矩阵
mat4 wAM = translate(vec3(A.x, A.y, A.z)); // 因为A为世界坐标，通过提取A的坐标组织初始世界变换矩阵
vec4 wB = wAM * B; // B点的初始世界坐标
mat4 wBM = wAM * translate(vec3(B.xyz)); // B点的世界变换矩阵
vec4 wC = wBM * C; // C点的初始世界坐标
float LA = length(wB.xyz - A.xyz); // A到B的长度（大臂长度）
float LB = length(wC.xyz - wB.xyz); // B到C的长度（小臂长度）
vec3 AT = target.xyz - A.xyz; // A到目标的向量
float LT = length(AT); // A到目标的距离
float diffTy = target.y - A.y; // 目标与A点的Y轴差值
float diffTx = target.x - A.x; // 目标与A点的X轴差值
float diffTz = target.z - A.z; // 目标与A点的Z轴差值
float zRotAngle = atan(diffTy, sqrt(diffTx * diffTx + diffTz * diffTz)); // 计算Z轴的第一个旋转角度对应O1
float yRotAngle = atan(diffTz, diffTx); // 计算Y轴旋转角度
// 如果目标距离超过机械臂的最大长度（不可到达）
if ((LA + LB) < LT) {
// 我们使用的左手坐标系，Y轴的旋转是顺时针旋转，旋转角度为yRotAngle的负角度
wAM *= rotateY(-yRotAngle) * rotateZ(zRotAngle); // 旋转A点的变换矩阵，使其指向目标
combineMat = wAM * translate(lB.xyz); // 组合变换矩阵
C = combineMat * lC; // 更新C点的位置
float rotateVal = iTime * 4.; // 动态旋转值（时间驱动）
mat4 jawMat = combineMat * translate(lC.xyz) * rotateX(rotateVal); // 计算夹爪的变换矩阵
jaw1 = jawMat * jaw1; // 更新夹爪1的位置
jaw2 = jawMat * jaw2; // 更新夹爪2的位置
B = wAM * lB; // 更新B点的位置
} else {
// 如果目标在机械臂的可到达范围内
float LA2 = LA * LA; // A到B的长度平方
float LT2 = LT * LT; // A到目标的长度平方
float LB2 = LB * LB; // B到C的长度平方
float z2RotAngle = acos((LA2 + LT2 - LB2) / (2. * LA * LT)); // 计算Z轴的第二个旋转角度对应O2
float zBRotAngle = -(PI - acos((LA2 + LB2 - LT2) / (2. * LA * LB))); // 计算B点的Z轴旋转角度
wAM *= rotateY(-yRotAngle) * rotateZ(zRotAngle + z2RotAngle); // 更新A点的变换矩阵
combineMat = wAM * translate(lB.xyz) * rotateZ(zBRotAngle); // 组合变换矩阵
C = combineMat * lC; // 更新C点的位置
float rotateVal = iTime * 4.; // 动态旋转值（时间驱动）
mat4 jawMat = combineMat * translate(lC.xyz) * rotateX(rotateVal); // 计算夹爪的变换矩阵
jaw1 = jawMat * jaw1; // 更新夹爪1的位置
jaw2 = jawMat * jaw2; // 更新夹爪2的位置
B = wAM * lB; // 更新B点的位置
}
}
Fabrik求解
Fabrik（Forward and Backward Reaching Inverse Kinematics）是一种高效的迭代逆运动学算法，通过前后传递调整关节位置，逐步逼近目标。它的核心思想是通过交替的反向迭代（从末端到基座）和正向迭代（从基座到末端）调整关节位置，保持关节长度不变，最终使末端执行器逼近目标位置。
Fabrik（Forward and Backward Reaching Inverse Kinematics）是一种高效的迭代逆运动学算法，通过前后传递调整关节位置，逐步逼近目标。以下是其详细介绍：
核心思想
Fabrik通过交替的反向迭代（从末端到基座）和正向迭代（从基座到末端）调整关节位置，保持关节长度不变，最终使末端执行器逼近目标位置。
算法步骤
-
初始化确定关节链的初始位置 ( {p_0, p_1, ..., p_n} )，其中 ( p_0 ) 为基座，( p_n ) 为末端，每个关节长度 ( l_i = |p_{i+1} - p_i| )。
-
迭代过程
-
反向迭代
- 将末端 ( p_n ) 移动到目标位置 ( t )。
- 从倒数第二个关节 ( p_{n-1} ) 开始，依次调整每个关节位置，使其与下一关节保持原长度：
[
p_i' = p_{i+1}' + \frac{p_i - p_{i+1}}{|p_i - p_{i+1}|} \cdot l_i
]
直至调整到基座 ( p_0 )。
-
正向迭代
- 固定基座 ( p_0 )，从第二个关节 ( p_1 ) 开始，依次调整位置以满足与前一个关节的原长度：
[
p_i'' = p_{i-1}'' + \frac{p_i' - p_{i-1}''}{|p_i' - p_{i-1}''|} \cdot l_{i-1}
]
直至调整到末端 ( p_n )。
- 固定基座 ( p_0 )，从第二个关节 ( p_1 ) 开始，依次调整位置以满足与前一个关节的原长度：
-
-
终止条件
重复迭代，直到末端与目标的距离小于阈值或达到最大迭代次数。
反向迭代
上述的整个过程是不是很像在拉大锯？就好像是基座与末端效应器的双人游戏。假设我们有如下的关节链：
其中 t 是我们的目标位置，p1是基座(第一个关节)，前向传递的过程即是把p4移至t后，链条相关关节联动的过程。
p4移动至t点后，因为关节间的距离不变，所以p3也会跟着变动。
继而所有关节跟着变动：
正向迭代
接着进行正向迭代，即把P1'的点移动至原来P1的位置，继而引起其它关节的整体联动。联动方式与上述过程类似：
这就是整个过程的核心思想。直到P4点与t点的距离在可接受的阈值内，即表示算法成功收敛。
// 定义所有关节
public joints: Node[] = [];
start() {
for (let i = 0; i < this.joints.length; i++) {
const joint = this.joints[i];
const worldPos = joint.worldPosition.clone();
const worldRot = joint.worldRotation.clone();
// 关节起始位置
this.jointStartPositions.push(worldPos);
// 当前位置
this.jointCurrPositions.push(worldPos);
// 关节起始旋转(四元数)
this.jointStartRots.push(worldRot);
if (i > 0) {
const prevPos = this.jointStartPositions[i - 1];
// 关节间的距离
const distance = Vec3.distance(worldPos, prevPos);
this.jointBoneLengths.push(distance);
// 关节链总长度
this.chainLength += distance;
const dir = Vec3.subtract(new Vec3(), worldPos, prevPos).normalize();
// 记录起始方向
this.jointStartDirs.push(dir);
}
}
// 末端效应器起始世界旋转值
this.jointEndStartRot = this.joints[this.joints.length - 1].worldRotation.clone();
// 目标起始世界旋转值
this.targetStartRot = this.target.worldRotation.clone();
// 末端效应器节点
this.currEndJoint = this.joints[this.joints.length - 1];
}
// 反向迭代
backward() {
// 迭代算法的第一步：反向运动学计算，计算每个关节的位置
// 这里的关节位置是指在世界坐标系下的关节位置
for(let i = this.joints.length - 1; i >= 0; i--) {
if(i === this.joints.length - 1) {
this.jointCurrPositions[i] = this.target.worldPosition.clone();
} else {
this.jointCurrPositions[i] = Vec3.add(new Vec3(), this.jointCurrPositions[i + 1], Vec3.subtract(new Vec3(), this.jointCurrPositions[i], this.jointCurrPositions[i + 1]).normalize().multiplyScalar(this.jointBoneLengths[i]));
}
}
}
// 前向迭代
forward() {
// 迭代算法的第二步：正向运动学算法
for(let i = 0; i < this.joints.length; i++) {
if(i===0) {
this.jointCurrPositions[i] = this.rootStartPos.clone();
} else {
this.jointCurrPositions[i] = Vec3.add(new Vec3(), this.jointCurrPositions[i - 1], Vec3.subtract(new Vec3(), this.jointCurrPositions[i], this.jointCurrPositions[i - 1]).normalize().multiplyScalar(this.jointBoneLengths[i - 1]));
}
}
}
// fabrik迭代算法，可以简单理解成拉锯算法
fabIteration(targetPos: Vec3) {
let endPos = this.jointCurrPositions[this.joints.length-1];
// 这里的 endPos 是当前关节链的末端位置，targetPos 是目标位置
let distance = Vec3.distance(endPos, targetPos);;
// 当前迭代次数
this.currIterations = 0;
// 末端效应器与目标位置的距离小于阈值，迭代次数大于指定值则跳出迭代
while(distance > this.precision && this.currIterations < this.maxIterations) {
this.currIterations++;
// 逆向迭代
this.backward();
// 前向迭代
this.forward();
// 迭代后记录末端效应器与目标位置的距离，用于判断算法是否收敛
distance = Vec3.distance(this.jointCurrPositions[this.joints.length-1], targetPos);
}
}
ikExe() {
for(let i = 1; i < this.joints.length; i++) {
const joint = this.joints[i-1];
const startDir = Vec3.subtract(new Vec3(), this.jointStartPositions[i], this.jointStartPositions[i-1]).normalize();
const currDir = Vec3.subtract(new Vec3(), this.jointCurrPositions[i], this.jointCurrPositions[i-1]).normalize();
const targetRot = Quat.rotationTo(new Quat(), startDir, currDir);
// 因为是世界旋转，所以要乘上自身关节的世界旋转
const newRot = Quat.multiply(new Quat(), targetRot, this.jointStartRots[i-1]);
joint.setWorldRotation(newRot);
}
}
update() {
const startPos = this.jointStartPositions[0];
const targetPos = this.target.worldPosition;
const currDistance = Vec3.distance(startPos, targetPos);
// 当整个关节链的长度小于，那么就直接伸直整个链条
if (currDistance > this.chainLength) {
const targetDir = Vec3.subtract(new Vec3(), targetPos, startPos).normalize();
for(let i = 1; i < this.joints.length; i++) {
// 不要使用joints数组里的具体节点的世界坐标进行向量加法运算，因为节点的世界坐标还没来得及更新会造成抖动,如果非要使用节点的世界坐标，需要调用下updateWorldTransform方法
// 节点的 `worldPosition` 是基于父节点的变换计算得出的。如果父节点或祖先节点的变换在当前帧中发生了变化，但引擎尚未完成世界坐标的更新，那么读取的 `worldPosition` 是上一帧的值。
this.jointCurrPositions[i] = Vec3.add(new Vec3(), this.jointCurrPositions[i-1], Vec3.multiplyScalar(new Vec3(), targetDir, this.jointBoneLengths[i-1]));
}
} else {
this.fabIteration(targetPos);
}
this.ikExe();
const tarWorldRot = this.target.worldRotation;
// da = b, so d = b * a^-1
// 这里的 a 是目标的起始世界旋转，b 是关节末端的起始世界旋转，d 是关节与目标的起始相对旋转, 目的是让关节末端的世界旋转相对目标位置的旋转维持不变
const offsetEndJoint = Quat.multiply(new Quat(), this.jointEndStartRot, Quat.invert(new Quat(), this.targetStartRot));
this.currEndJoint.setWorldRotation(Quat.multiply(new Quat(), tarWorldRot, offsetEndJoint));
}
局限性
- 关节约束：基础算法不直接处理旋转限制，需额外扩展。
- 局部最优：复杂场景可能陷入局部最优，需合理初始化，如蜘蛛腿关节IK实现，在蜘蛛移动时需要重新获取初始方向等。
对比其他方法
- CCD（循环坐标下降法）：逐关节调整旋转角，收敛较慢；Fabrik整体调整位置，效率更高。
- 雅可比矩阵法：依赖矩阵运算，计算量大；Fabrik无矩阵操作，实现简单。
Fabrik以其简洁性和高效性，成为逆运动学领域的实用算法。
相关原理或代码如下:
