inverse-kinematics
Version:
Inverse kinematics for 2D and 3D applications
135 lines (134 loc) • 5 kB
JavaScript
var __spreadArray = (this && this.__spreadArray) || function (to, from) {
for (var i = 0, il = from.length, j = to.length; i < il; i++, j++)
to[j] = from[i];
return to;
};
import * as V3O from './V3O';
import * as MathUtils from './MathUtils';
export var multiply = function (a, b) {
var qax = a[0];
var qay = a[1];
var qaz = a[2];
var qaw = a[3];
var qbx = b[0];
var qby = b[1];
var qbz = b[2];
var qbw = b[3];
return [
qax * qbw + qaw * qbx + qay * qbz - qaz * qby,
qay * qbw + qaw * qby + qaz * qbx - qax * qbz,
qaz * qbw + qaw * qbz + qax * qby - qay * qbx,
qaw * qbw - qax * qbx - qay * qby - qaz * qbz,
];
};
export var fromEulerAngles = function (_a) {
var x = _a[0], y = _a[1], z = _a[2];
var cos = Math.cos;
var sin = Math.sin;
var c1 = cos(x / 2);
var c2 = cos(y / 2);
var c3 = cos(z / 2);
var s1 = sin(x / 2);
var s2 = sin(y / 2);
var s3 = sin(z / 2);
return [
s1 * c2 * c3 + c1 * s2 * s3,
c1 * s2 * c3 - s1 * c2 * s3,
c1 * c2 * s3 + s1 * s2 * c3,
c1 * c2 * c3 - s1 * s2 * s3,
];
};
export var slerp = function (from, to, amount) {
// Calculate angle between them.
var cosHalfTheta = from[0] * to[0] + from[1] * to[1] + from[2] * to[2] + from[3] * to[3];
// Are parallel in either direction. from = to || from = -to
if (Math.abs(cosHalfTheta) >= 1.0) {
return from;
}
var halfTheta = Math.acos(cosHalfTheta);
var sinHalfTheta = Math.sqrt(1.0 - cosHalfTheta * cosHalfTheta);
// if theta = 180 degrees then result is not fully defined
// we could rotate around any axis normal to qa or qb
if (Math.abs(sinHalfTheta) < 0.001) {
return [
from[0] * 0.5 + to[0] * 0.5,
from[1] * 0.5 + to[1] * 0.5,
from[2] * 0.5 + to[2] * 0.5,
from[3] * 0.5 + to[3] * 0.5,
];
}
var ratioA = Math.sin((1 - amount) * halfTheta) / sinHalfTheta;
var ratioB = Math.sin(amount * halfTheta) / sinHalfTheta;
//calculate Quaternion.
return [
from[0] * ratioA + to[0] * ratioB,
from[1] * ratioA + to[1] * ratioB,
from[2] * ratioA + to[2] * ratioB,
from[3] * ratioA + to[3] * ratioB,
];
};
export var conjugate = function (quaternion) {
return [-quaternion[0], -quaternion[1], -quaternion[2], quaternion[3]];
};
export var inverse = function (quaternion) {
var conj = conjugate(quaternion);
var mag = magnitude(quaternion);
return [conj[0] / mag, conj[1] / mag, conj[2] / mag, conj[3] / mag];
};
export var magnitude = function (quaternion) { return Math.hypot.apply(Math, quaternion); };
export var zeroRotation = function () { return [0, 0, 0, 1]; };
export var normalize = function (quaternion) {
var length = Math.hypot.apply(Math, quaternion);
if (length === 0)
return zeroRotation();
return [quaternion[0] / length, quaternion[1] / length, quaternion[2] / length, quaternion[3] / length];
};
export var clamp = function (quaternion, lowerBound, upperBound) {
var rotationAxis = [quaternion[0], quaternion[1], quaternion[2]];
var w = quaternion[3];
var _a = V3O.fromArray(rotationAxis.map(function (component, index) {
var angle = 2 * Math.atan(component / w);
var lower = lowerBound[index];
var upper = upperBound[index];
if (lower > upper)
throw new Error("Lower bound should be less than upper bound for component " + index + ". Lower: " + lower + ", upper: " + upper);
var clampedAngle = MathUtils.clamp(angle, lower, upper);
return Math.tan(0.5 * clampedAngle);
})), x = _a[0], y = _a[1], z = _a[2];
return normalize([x, y, z, 1]);
};
export var fromUnitDirectionVector = function (vector) {
return rotationFromTo([1, 0, 0], vector);
};
export var rotationFromTo = function (a, b) {
var aNormalised = V3O.normalise(a);
var bNormalised = V3O.normalise(b);
var dot = V3O.dotProduct(aNormalised, bNormalised);
var isParallel = dot >= 1;
if (isParallel) {
// a, b are parallel
return zeroRotation();
}
var isAntiParallel = dot < -1 + Number.EPSILON;
if (isAntiParallel) {
var axis = V3O.crossProduct([1, 0, 0], aNormalised);
var aPointsForward = V3O.sqrEuclideanLength(axis) === 0;
if (aPointsForward) {
axis = V3O.crossProduct([0, 1, 0], aNormalised);
}
axis = V3O.normalise(axis);
return fromAxisAngle(axis, Math.PI);
}
var q = __spreadArray(__spreadArray([], V3O.crossProduct(aNormalised, bNormalised)), [1 + dot]);
return normalize(q);
};
export var fromAxisAngle = function (axis, angle) {
var halfAngle = angle / 2;
return __spreadArray(__spreadArray([], V3O.scale(axis, Math.sin(halfAngle))), [Math.cos(halfAngle)]);
};
export var fromObject = function (object) { return [
object.x,
object.y,
object.z,
object.w,
]; };