mirror of
https://github.com/Dingo-Shenanigans/ReSkate.git
synced 2026-10-23 17:35:57 +02:00
AyeZeeBB
118c489fdb
PR 343's Filmer becomes a Luau plugin mod (kept in contrib/plugins/filmer) on general APIs instead of
its own C++:
- reskate.skater.carry: something held out in one or both hands and aimed, the body crouched (feet
kept) and hunched over it, turned on the spot after it (Extension/Skater/carry.cpp, from the PR's
filmer_hold.cpp, with tests)
- shown = "everyone" | "others" | "me" on skater.pose, reach and carry: the pose sent to the other
players and the one drawn here are split when asked
- reskate.camera.cycle and the camera_cycle event: a plugin's camera as the last step of the camera
height button's cycle, after first person
- reskate.session.nametags(false) holds the names over the other players off
- board.hide(bone, {others = true}) hides it in the board the others are sent too (a sent-pose filter)
- outfit.set takes hand wear slots; input.bind takes an action with no key of its own
466 lines
27 KiB
C++
466 lines
27 KiB
C++
#include "carry.h"
|
|
#include <algorithm>
|
|
#include <cmath>
|
|
|
|
namespace dingosdk::carry {
|
|
namespace {
|
|
using anim_studio::conjugate;
|
|
using anim_studio::multiply;
|
|
using anim_studio::normalised;
|
|
using anim_studio::rotate;
|
|
using arm_reach::chain;
|
|
using arm_reach::compose;
|
|
|
|
Vec3 add(const Vec3& a, const Vec3& b) { return {a[0] + b[0], a[1] + b[1], a[2] + b[2]}; }
|
|
Vec3 sub(const Vec3& a, const Vec3& b) { return {a[0] - b[0], a[1] - b[1], a[2] - b[2]}; }
|
|
Vec3 scaled(const Vec3& a, float k) { return {a[0] * k, a[1] * k, a[2] * k}; }
|
|
float dot(const Vec3& a, const Vec3& b) { return a[0] * b[0] + a[1] * b[1] + a[2] * b[2]; }
|
|
Vec3 cross(const Vec3& a, const Vec3& b) { return {a[1] * b[2] - a[2] * b[1], a[2] * b[0] - a[0] * b[2], a[0] * b[1] - a[1] * b[0]}; }
|
|
float length(const Vec3& a) { return std::sqrt(dot(a, a)); }
|
|
Vec3 unit(const Vec3& a, const Vec3& fallback) {
|
|
const float l = length(a);
|
|
return l > 1e-6f && std::isfinite(l) ? scaled(a, 1.0f / l) : fallback;
|
|
}
|
|
bool finite(const Vec3& v) { return std::isfinite(v[0]) && std::isfinite(v[1]) && std::isfinite(v[2]); }
|
|
bool finite(const Quat& q) { return std::isfinite(q[0]) && std::isfinite(q[1]) && std::isfinite(q[2]) && std::isfinite(q[3]); }
|
|
// The shortest turn taking direction `from` onto direction `to`.
|
|
Quat arc(const Vec3& from, const Vec3& to) {
|
|
const Vec3 a = unit(from, {0, 0, 1}), b = unit(to, {0, 0, 1});
|
|
const float d = dot(a, b);
|
|
if (d < -0.9999f) { // half a turn: about any axis square to `from`
|
|
Vec3 axis = cross({1, 0, 0}, a);
|
|
if (length(axis) < 1e-3f) axis = cross({0, 1, 0}, a);
|
|
axis = unit(axis, {0, 1, 0});
|
|
return {axis[0], axis[1], axis[2], 0.0f};
|
|
}
|
|
const Vec3 c = cross(a, b);
|
|
return normalised({c[0], c[1], c[2], 1.0f + d});
|
|
}
|
|
// Part way from a to b, the short way round.
|
|
Quat blend(const Quat& a, Quat b, float t) {
|
|
t = std::clamp(t, 0.0f, 1.0f);
|
|
if (a[0] * b[0] + a[1] * b[1] + a[2] * b[2] + a[3] * b[3] < 0.0f)
|
|
for (auto& v : b) v = -v;
|
|
return normalised({a[0] + (b[0] - a[0]) * t, a[1] + (b[1] - a[1]) * t, a[2] + (b[2] - a[2]) * t, a[3] + (b[3] - a[3]) * t});
|
|
}
|
|
Quat about_up(float radians) { return {0.0f, std::sin(radians / 2), 0.0f, std::cos(radians / 2)}; }
|
|
// The world turn `world` as the local rotation of a joint whose parent is placed at `parent`.
|
|
Quat local_of(const Placed& parent, const Quat& world) { return normalised(multiply(conjugate(parent.rotation), world)); }
|
|
// The way an aimed thing faces round the upright (level, unit), steady however steeply it points up or
|
|
// down: straight down, its forward has no level part left, but its up does (the top of it is the way
|
|
// it faces).
|
|
Vec3 level_heading(const Quat& aim) {
|
|
const Quat q = normalised(aim);
|
|
const Vec3 forward = rotate(q, {0.0f, 0.0f, -1.0f}), up = rotate(q, {0.0f, 1.0f, 0.0f});
|
|
const float sin_p = std::clamp(forward[1], -1.0f, 1.0f), cos_p = std::sqrt(std::max(0.0f, 1.0f - sin_p * sin_p));
|
|
return unit(Vec3{forward[0] * cos_p - up[0] * sin_p, 0.0f, forward[2] * cos_p - up[2] * sin_p}, {0.0f, 0.0f, 1.0f});
|
|
}
|
|
|
|
// In an arm's chain: chest, shoulder, upper arm, forearm, hand.
|
|
constexpr std::size_t chest = 6, shoulder = 7, upper = 8, fore = 9, hand_at = 10;
|
|
// Held, not worn: from the wrist joint to the middle of the palm along the hand.
|
|
constexpr float palm_from_wrist = 0.075f;
|
|
// Where the other hand takes it, in the item's own frame (right, up, back; the grip at the origin), for
|
|
// a right-hand grip: the left side of it, underneath. Mirrored for a left grip.
|
|
constexpr Vec3 support_from_grip{-0.08f, -0.16f, -0.04f};
|
|
// The body as a column the arm keeps out of: about the hips' and chest's line, this far out at most
|
|
// (and inside the shoulders).
|
|
constexpr float body_radius = 0.24f;
|
|
// Its front is held at least this far out in front of the shoulder (along the aim), and at least this
|
|
// far below it.
|
|
constexpr float min_reach_out = 0.4f, below_shoulder = 0.05f;
|
|
// Toward the holding hand's own side it comes up this much (all the way square to that side).
|
|
constexpr float side_lift = 0.2f;
|
|
|
|
// One foot lifted `up` metres and moved by `move` (a step), the knee bending to it the way it already
|
|
// bends; the foot keeps its turn.
|
|
bool step_foot(std::span<Joint> pose, std::span<const std::uint16_t> path, float up, const Vec3& move) noexcept {
|
|
std::array<Placed, 6> now{};
|
|
if (!compose(pose, path, now)) return false;
|
|
const Vec3 hip = now[3].position, knee = now[4].position;
|
|
const Vec3 ankle = add(now[5].position, add(Vec3{0.0f, up, 0.0f}, move));
|
|
const float a = length(sub(knee, hip)), b = length(sub(now[5].position, knee));
|
|
if (!(a > 0.1f && b > 0.1f)) return true;
|
|
const float d = std::clamp(length(sub(ankle, hip)), std::abs(a - b) + 0.01f, (a + b) * 0.999f);
|
|
const Vec3 along = unit(sub(ankle, hip), {0, -1, 0});
|
|
Vec3 pole = sub(knee, add(hip, scaled(along, dot(sub(knee, hip), along))));
|
|
pole = unit(pole, Vec3{0.0f, 0.0f, 1.0f});
|
|
const float cos_a = std::clamp((a * a + d * d - b * b) / (2.0f * a * d), -1.0f, 1.0f);
|
|
const float sin_a = std::sqrt(std::max(0.0f, 1.0f - cos_a * cos_a));
|
|
const Vec3 new_knee = add(hip, add(scaled(along, a * cos_a), scaled(pole, a * sin_a)));
|
|
const Quat foot_world = now[5].rotation;
|
|
const Quat thigh_world = normalised(multiply(arc(sub(knee, hip), sub(new_knee, hip)), now[3].rotation));
|
|
pose[path[3]].rotation = local_of(now[2], thigh_world);
|
|
if (!compose(pose, path, now)) return false;
|
|
const Quat shin_world = normalised(multiply(arc(sub(now[5].position, now[4].position), sub(ankle, now[4].position)), now[4].rotation));
|
|
pose[path[4]].rotation = local_of(now[3], shin_world);
|
|
if (!compose(pose, path, now)) return false;
|
|
pose[path[5]].rotation = local_of(now[4], foot_world);
|
|
return true;
|
|
}
|
|
|
|
struct One {
|
|
Hand hand = Hand::right;
|
|
Vec3 front{};
|
|
Quat aim{0, 0, 0, 1};
|
|
Vec3 front_from_grip{};
|
|
std::optional<Grip> worn;
|
|
float weight = 1.0f;
|
|
// The front reached for where it is (no holding it out in front and below the shoulder): the
|
|
// supporting hand's reach.
|
|
bool free{};
|
|
};
|
|
|
|
// One arm holding the item (its upper arm and forearm, and the hand when it is worn).
|
|
Held hold_one(std::span<Joint> pose, const One& goal) noexcept {
|
|
const auto joints = chain(goal.hand);
|
|
std::array<Placed, arm_reach::right_chain.size()> w{};
|
|
if (!compose(pose, joints, w)) return {};
|
|
const Quat aim = normalised(goal.aim);
|
|
const Vec3 s = w[upper].position, e = w[fore].position, h = w[hand_at].position;
|
|
const float a = length(sub(e, s)), b = length(sub(h, e));
|
|
if (!(a > 0.05f && b > 0.05f && a < 1.0f && b < 1.0f)) return {};
|
|
const float scale = std::max(w[hand_at].scale, 0.1f);
|
|
// Where its front is shown held: where it was asked for, but out in front of the shoulder along
|
|
// the aim and no higher than the shoulder, as a person holds a thing they point (a front up by the
|
|
// head or tucked against the body would fold the arm up into the face).
|
|
Vec3 front = goal.front;
|
|
float to_own_side = 0.0f;
|
|
if (!goal.free) {
|
|
const Vec3 ahead = level_heading(aim);
|
|
const float out = dot(sub(front, s), ahead);
|
|
// Toward the holding hand's own side the arm would stretch across to its full length: it comes
|
|
// in closer and up a little there, the elbow bent.
|
|
Vec3 own_side = sub(s, w[chest].position);
|
|
own_side[1] = 0.0f;
|
|
own_side = unit(own_side, ahead);
|
|
const float toward_side = std::clamp(dot(ahead, own_side), 0.0f, 1.0f);
|
|
to_own_side = toward_side;
|
|
// Where it is held follows the aim in one smooth sweep: level, out in front at the chest, under
|
|
// the eyes; aimed down, lower and a little closer, all the way down to the stomach; aimed up,
|
|
// higher and closer to the face, up over the head.
|
|
const float pointing_y = std::clamp(rotate(aim, {0.0f, 0.0f, -1.0f})[1], -1.0f, 1.0f);
|
|
const float up_part = std::max(pointing_y, 0.0f), down_part = std::max(-pointing_y, 0.0f);
|
|
const float height = -below_shoulder - 0.13f * (1.0f - up_part) + 0.55f * up_part - 0.45f * down_part;
|
|
const float reach_out = (min_reach_out + 0.08f * (1.0f - up_part)) * (1.0f - 0.35f * toward_side) * (1.0f - 0.55f * up_part) *
|
|
(1.0f - 0.15f * down_part);
|
|
const Vec3 across = cross({0.0f, 1.0f, 0.0f}, ahead);
|
|
const float lateral = std::clamp(dot(sub(front, s), across), -0.3f, 0.3f);
|
|
const float out_now = std::max(out, 0.0f);
|
|
// (Further out than that only toward the other side, across the body.)
|
|
const float out_held = toward_side > 0.0f && out_now > reach_out + 0.1f ? out_now - (out_now - reach_out - 0.1f) * toward_side : reach_out;
|
|
front = add(add(s, scaled(ahead, out_held)), scaled(across, lateral));
|
|
front[1] = s[1] + height + side_lift * toward_side;
|
|
}
|
|
const Grip worn = goal.worn.value_or(Grip{});
|
|
const Quat hand_world = normalised(multiply(aim, conjugate(worn.item)));
|
|
const Vec3 grip_offset = scaled(rotate(hand_world, worn.from_wrist), scale);
|
|
Vec3 palm_goal = goal.worn ? sub(sub(front, rotate(aim, goal.front_from_grip)), grip_offset) : sub(front, rotate(aim, goal.front_from_grip));
|
|
if (!goal.free) {
|
|
// The hand itself out in front of the shoulder and not up over it: with the hand tucked in over
|
|
// the shoulder the arm can only fold up, the elbow rising by the head. Aimed up it comes up and
|
|
// in a little, smoothly with the tilt.
|
|
const Vec3 forward = level_heading(aim);
|
|
const float up_part = std::clamp(rotate(aim, {0.0f, 0.0f, -1.0f})[1], 0.0f, 1.0f);
|
|
const float out_min = (0.24f - 0.06f * up_part) * (1.0f - 0.35f * to_own_side);
|
|
const float out_now = dot(sub(palm_goal, s), forward);
|
|
if (out_now < out_min) palm_goal = add(palm_goal, scaled(forward, out_min - out_now));
|
|
const float top = s[1] + 0.04f + 0.22f * up_part + side_lift * to_own_side;
|
|
if (palm_goal[1] > top) palm_goal[1] = top;
|
|
}
|
|
{
|
|
// The body as a column through the hips and chest: the hand's target, and the straight reach to
|
|
// it from the shoulder, kept out of it (pushed out along the way they came closest).
|
|
const Vec3 low = w[2].position, high = w[chest].position;
|
|
const float bottom = low[1] - 0.35f, top = high[1] + 0.15f;
|
|
const auto axis_at = [&](float y) {
|
|
const float t = std::clamp((y - low[1]) / std::max(high[1] - low[1], 0.05f), 0.0f, 1.0f);
|
|
return Vec3{low[0] + (high[0] - low[0]) * t, y, low[2] + (high[2] - low[2]) * t};
|
|
};
|
|
// As wide as the body is: inside the shoulders (the arm hangs from the column's edge).
|
|
Vec3 shoulder_out = sub(s, axis_at(s[1]));
|
|
shoulder_out[1] = 0.0f;
|
|
const float radius = std::min(body_radius, length(shoulder_out) * 0.85f);
|
|
for (int round = 0; round < 3; ++round) {
|
|
float nearest = radius;
|
|
Vec3 out_dir{};
|
|
for (int k = 3; k <= 8; ++k) {
|
|
const Vec3 p = add(s, scaled(sub(palm_goal, s), static_cast<float>(k) / 8.0f));
|
|
if (p[1] < bottom || p[1] > top) continue;
|
|
Vec3 radial = sub(p, axis_at(p[1]));
|
|
radial[1] = 0.0f;
|
|
const float r = length(radial);
|
|
if (r < nearest) {
|
|
nearest = r;
|
|
Vec3 side_out = sub(s, w[chest].position);
|
|
side_out[1] = 0.0f;
|
|
out_dir = unit(radial, unit(side_out, {goal.hand == Hand::left ? -1.0f : 1.0f, 0, 0}));
|
|
}
|
|
}
|
|
if (nearest >= radius) break;
|
|
palm_goal = add(palm_goal, scaled(out_dir, radius - nearest + 0.01f));
|
|
}
|
|
}
|
|
const float palm = goal.worn ? 0.0f : palm_from_wrist * scale, b_palm = b + palm;
|
|
// Never held at full stretch: the elbow keeps a bend (it comes in a little rather than the arm
|
|
// locking straight).
|
|
const float reach = (a + b_palm) * (goal.free ? 0.995f : 0.93f);
|
|
const float wanted = length(sub(palm_goal, s));
|
|
const float d = std::clamp(wanted, std::abs(a - b_palm) + 0.02f, reach);
|
|
const Vec3 direction = unit(sub(palm_goal, s), {0, -1, 0});
|
|
const Vec3 target = add(s, scaled(direction, d));
|
|
// The elbow out from the body and a little down and back: the shoulder's side of the chest.
|
|
Vec3 side = sub(s, w[chest].position);
|
|
side[1] = 0.0f;
|
|
side = unit(side, {goal.hand == Hand::left ? -1.0f : 1.0f, 0, 0});
|
|
// (Forward for the elbow: the way the arm reaches, level; reaching nearly straight up it has almost
|
|
// none, and would flip about, so it leans on the way the item points then.)
|
|
const Vec3 forward = level_heading(aim);
|
|
Vec3 ahead{direction[0], 0.0f, direction[2]};
|
|
ahead = add(ahead, scaled(forward, std::max(0.0f, 0.6f - length(ahead))));
|
|
ahead = unit(ahead, forward);
|
|
Vec3 pole = add(add(scaled(side, 0.8f), Vec3{0.0f, -0.45f, 0.0f}), scaled(ahead, -0.35f));
|
|
{
|
|
// Raised (up to the eye, up over the head): the elbow comes round to the front and a little out,
|
|
// as an arm does lifting something up in front of the face, gradually with the aim; never back
|
|
// behind the shoulder, never winged straight out.
|
|
const float lifted = std::clamp((rotate(aim, {0.0f, 0.0f, -1.0f})[1] - 0.1f) / 0.8f, 0.0f, 1.0f);
|
|
const float ease_in = lifted * lifted * (3.0f - 2.0f * lifted);
|
|
if (ease_in > 0.0f) {
|
|
const Vec3 lifted_pole = add(add(scaled(side, 0.85f), Vec3{0.0f, -0.4f, 0.0f}), scaled(forward, 0.35f));
|
|
pole = add(scaled(pole, 1.0f - ease_in), scaled(lifted_pole, ease_in));
|
|
}
|
|
}
|
|
pole = sub(pole, scaled(direction, dot(pole, direction)));
|
|
{
|
|
// The elbow hangs: whatever way the arm reaches, it bends with the elbow below the line from
|
|
// shoulder to hand (and out to the side), never folding up over the shoulder. (Reaching up high,
|
|
// straight down from that line would put it back behind the shoulder: there it only comes down
|
|
// as far as it can without going back.)
|
|
Vec3 hang = sub(Vec3{0.0f, -1.0f, 0.0f}, scaled(direction, -direction[1]));
|
|
const float back = dot(hang, forward);
|
|
if (back < 0.0f) hang = sub(hang, scaled(forward, back));
|
|
hang = sub(hang, scaled(direction, dot(hang, direction)));
|
|
const float hang_len = length(hang);
|
|
if (hang_len > 0.05f) pole = add(unit(pole, side), scaled(hang, 0.6f / hang_len));
|
|
}
|
|
pole = unit(pole, side);
|
|
const float cos_a = std::clamp((a * a + d * d - b_palm * b_palm) / (2.0f * a * d), -1.0f, 1.0f);
|
|
const float sin_a = std::sqrt(std::max(0.0f, 1.0f - cos_a * cos_a));
|
|
const Vec3 elbow = add(s, add(scaled(direction, a * cos_a), scaled(pole, a * sin_a)));
|
|
const Vec3 wrist = add(elbow, scaled(unit(sub(target, elbow), direction), b));
|
|
// The upper arm turned onto the new elbow, then the forearm (as the upper arm left it) onto the wrist.
|
|
const Quat turn_upper = arc(sub(e, s), sub(elbow, s));
|
|
const Quat upper_world = normalised(multiply(turn_upper, w[upper].rotation));
|
|
const Quat turn_fore = arc(rotate(turn_upper, sub(h, e)), sub(wrist, elbow));
|
|
const Quat fore_world = normalised(multiply(turn_fore, multiply(turn_upper, w[fore].rotation)));
|
|
// Back to each joint's own rotation, part way by the weight.
|
|
const float weight = std::clamp(goal.weight, 0.0f, 1.0f);
|
|
auto& upper_bone = pose[joints[upper]];
|
|
auto& fore_bone = pose[joints[fore]];
|
|
upper_bone.rotation = blend(upper_bone.rotation, normalised(multiply(conjugate(w[shoulder].rotation), upper_world)), weight);
|
|
fore_bone.rotation = blend(fore_bone.rotation, normalised(multiply(conjugate(upper_world), fore_world)), weight);
|
|
if (goal.worn) {
|
|
// The hand, from the forearm as it now is, turned so the worn item points along the aim.
|
|
if (!compose(pose, joints, w)) return {};
|
|
auto& hand_bone = pose[joints[hand_at]];
|
|
hand_bone.rotation = blend(hand_bone.rotation, normalised(multiply(conjugate(w[fore].rotation), hand_world)), weight);
|
|
}
|
|
// Where the hand ended up, from the pose as it now is.
|
|
if (!compose(pose, joints, w)) return {};
|
|
Held held;
|
|
if (goal.worn) {
|
|
held.item.rotation = normalised(multiply(w[hand_at].rotation, worn.item));
|
|
held.item.position = add(w[hand_at].position, scaled(rotate(w[hand_at].rotation, worn.from_wrist), scale));
|
|
} else {
|
|
// Held: the grip lies on the forearm's line, past the wrist.
|
|
const Vec3 along = unit(sub(w[hand_at].position, w[fore].position), {0, -1, 0});
|
|
held.item.position = add(w[hand_at].position, scaled(along, palm_from_wrist * scale));
|
|
held.item.rotation = aim;
|
|
}
|
|
held.front = add(held.item.position, rotate(held.item.rotation, goal.front_from_grip));
|
|
held.valid = finite(held.item.position) && finite(held.front);
|
|
held.reached = wanted <= reach + 0.01f;
|
|
return held;
|
|
}
|
|
} // namespace
|
|
|
|
bool crouch(std::span<Joint> pose, float depth, float twist) noexcept {
|
|
if (!((depth > 0.001f || std::abs(twist) > 0.001f) && std::isfinite(depth) && std::isfinite(twist))) return true;
|
|
depth = std::clamp(depth, 0.0f, 0.6f);
|
|
struct Leg {
|
|
std::span<const std::uint16_t> path;
|
|
std::array<Placed, 6> before{};
|
|
};
|
|
std::array<Leg, 2> legs{{{right_leg}, {left_leg}}};
|
|
for (auto& leg : legs)
|
|
if (!compose(pose, leg.path, leg.before)) return false;
|
|
// The hips down, in their parent's frame (the skater's placement).
|
|
const auto& parent = legs[0].before[1];
|
|
auto& hips = pose[hips_joint];
|
|
const Vec3 drop = rotate(conjugate(parent.rotation), Vec3{0.0f, -depth, 0.0f});
|
|
hips.position = add(hips.position, scaled(drop, 1.0f / std::max(parent.scale, 0.1f)));
|
|
// Turned round the upright, the feet staying where they are.
|
|
if (std::abs(twist) > 0.001f) {
|
|
std::array<Placed, 6> now{};
|
|
if (!compose(pose, legs[0].path, now)) return false;
|
|
hips.rotation = local_of(parent, normalised(multiply(about_up(twist), now[2].rotation)));
|
|
}
|
|
for (auto& leg : legs) {
|
|
std::array<Placed, 6> now{};
|
|
if (!compose(pose, leg.path, now)) return false;
|
|
const Vec3 hip = now[3].position, knee = leg.before[4].position, ankle = leg.before[5].position;
|
|
const float a = length(sub(leg.before[4].position, leg.before[3].position)), b = length(sub(ankle, knee));
|
|
if (!(a > 0.1f && b > 0.1f)) continue;
|
|
const float d = std::clamp(length(sub(ankle, hip)), std::abs(a - b) + 0.01f, (a + b) * 0.999f);
|
|
const Vec3 along = unit(sub(ankle, hip), {0, -1, 0});
|
|
// The knee bends the way it already does (forward of the hip-to-ankle line).
|
|
const Vec3 old_line = unit(sub(ankle, leg.before[3].position), along);
|
|
const Vec3 old_mid = add(leg.before[3].position, scaled(old_line, dot(sub(knee, leg.before[3].position), old_line)));
|
|
Vec3 pole = sub(knee, old_mid);
|
|
pole = sub(pole, scaled(along, dot(pole, along)));
|
|
pole = unit(pole, Vec3{0.0f, 0.0f, 1.0f});
|
|
const float cos_a = std::clamp((a * a + d * d - b * b) / (2.0f * a * d), -1.0f, 1.0f);
|
|
const float sin_a = std::sqrt(std::max(0.0f, 1.0f - cos_a * cos_a));
|
|
const Vec3 new_knee = add(hip, add(scaled(along, a * cos_a), scaled(pole, a * sin_a)));
|
|
// Thigh, then shin, turned onto the new knee and the ankle; the foot keeps its turn in the world.
|
|
const Quat thigh_world = normalised(multiply(arc(sub(now[4].position, hip), sub(new_knee, hip)), now[3].rotation));
|
|
pose[leg.path[3]].rotation = local_of(now[2], thigh_world);
|
|
if (!compose(pose, leg.path, now)) return false;
|
|
const Quat shin_world = normalised(multiply(arc(sub(now[5].position, now[4].position), sub(ankle, now[4].position)), now[4].rotation));
|
|
pose[leg.path[4]].rotation = local_of(now[3], shin_world);
|
|
if (!compose(pose, leg.path, now)) return false;
|
|
pose[leg.path[5]].rotation = local_of(now[4], leg.before[5].rotation);
|
|
}
|
|
return true;
|
|
}
|
|
|
|
bool hunch(std::span<Joint> pose, const Vec3& toward, float angle, float look, float twist) noexcept {
|
|
if (!(std::abs(angle) + std::abs(look) + std::abs(twist) > 1e-4f) || !finite(toward)) return true;
|
|
Vec3 level{toward[0], 0.0f, toward[2]};
|
|
if (length(level) < 1e-4f) return true;
|
|
level = unit(level, {0, 0, 1});
|
|
// Turning about this bends the top of the body over toward `level`.
|
|
const Vec3 axis = unit(cross(Vec3{0.0f, 1.0f, 0.0f}, level), {1, 0, 0});
|
|
// Spine, Spine1, Spine2, Spine3 share the back's angle; Neck, Neck1 and Head the look.
|
|
constexpr std::array<float, 7> share{0.15f, 0.25f, 0.3f, 0.3f, 0.35f, 0.25f, 0.4f};
|
|
constexpr std::array<float, 7> twist_share{0.1f, 0.1f, 0.15f, 0.2f, 0.15f, 0.1f, 0.2f};
|
|
for (std::size_t i = 0; i < back_joints.size(); ++i) {
|
|
std::array<Placed, head_chain.size()> w{};
|
|
if (!compose(pose, head_chain, w)) return false;
|
|
const std::size_t at = 3 + i; // its place in head_chain
|
|
const float turn = (i < 4 ? angle : look) * share[i];
|
|
const Quat bend{axis[0] * std::sin(turn / 2), axis[1] * std::sin(turn / 2), axis[2] * std::sin(turn / 2), std::cos(turn / 2)};
|
|
const Quat world = normalised(multiply(bend, multiply(about_up(twist * twist_share[i]), w[at].rotation)));
|
|
pose[back_joints[i]].rotation = local_of(w[at - 1], world);
|
|
}
|
|
return true;
|
|
}
|
|
|
|
Held carry(std::span<Joint> pose, const Goal& goal) noexcept {
|
|
if (pose.size() <= 343 || !finite(goal.front) || !finite(goal.aim) || !finite(goal.front_from_grip)) return {};
|
|
const float body_weight = std::clamp(goal.weight, 0.0f, 1.0f);
|
|
if (std::abs(goal.turn) > 1e-4f && std::isfinite(goal.turn)) {
|
|
// The whole body turned round the upright through the hips: the hips (everything hangs from
|
|
// them) turned in place.
|
|
std::array<Placed, 3> w3{};
|
|
if (!compose(pose, std::span<const std::uint16_t>(right_leg).first(3), w3)) return {};
|
|
pose[hips_joint].rotation = local_of(w3[1], normalised(multiply(about_up(goal.turn * body_weight), w3[2].rotation)));
|
|
}
|
|
// Turning on the spot: the feet step round with it, one then the other, each lifted a little and
|
|
// set down a little in under the hips.
|
|
if (goal.step > 0.01f && std::isfinite(goal.step_phase)) {
|
|
const float phase = goal.step_phase - std::floor(goal.step_phase);
|
|
const bool right_foot = phase < 0.5f;
|
|
const float half = right_foot ? phase * 2.0f : (phase - 0.5f) * 2.0f;
|
|
const float lift = std::sin(half * 3.14159265f) * 0.09f * std::clamp(goal.step, 0.0f, 1.0f) * body_weight;
|
|
std::array<Placed, 6> leg{};
|
|
const auto path = right_foot ? std::span<const std::uint16_t>(right_leg) : std::span<const std::uint16_t>(left_leg);
|
|
if (lift > 0.002f && compose(pose, path, leg)) {
|
|
Vec3 in = sub(leg[2].position, leg[5].position);
|
|
in[1] = 0.0f;
|
|
const float inward = std::min(length(in), 0.06f) * std::sin(half * 3.14159265f);
|
|
if (!step_foot(pose, path, lift, scaled(unit(in, {0, 0, 0}), inward * 0.5f))) return {};
|
|
}
|
|
}
|
|
// Which way it points against the way the body faces (round the upright), for the body turning to
|
|
// it: the hips a quarter, the back and shoulders and head the rest.
|
|
float full = 0.0f, pitch = 0.0f;
|
|
Vec3 facing{0, 0, 1};
|
|
if (goal.hunch > 0.0f || goal.crouch > 0.0f) {
|
|
facing = body_facing(pose);
|
|
const Vec3 level = level_heading(goal.aim);
|
|
full = std::clamp(std::atan2(dot(cross(facing, level), {0.0f, 1.0f, 0.0f}), dot(facing, level)), -1.6f, 1.6f);
|
|
pitch = std::asin(std::clamp(rotate(normalised(goal.aim), {0.0f, 0.0f, -1.0f})[1], -1.0f, 1.0f)); // down is negative
|
|
}
|
|
if (goal.still)
|
|
for (std::size_t i = 0; i < back_joints.size(); ++i)
|
|
pose[back_joints[i]].rotation = blend(pose[back_joints[i]].rotation, standing_back[i], body_weight);
|
|
constexpr float hips_share = 0.25f;
|
|
const bool turning = goal.hunch > 0.0f;
|
|
if ((goal.crouch > 0.0f || turning) && !crouch(pose, goal.crouch * body_weight, turning ? full * hips_share * body_weight : 0.0f)) return {};
|
|
if (turning) {
|
|
// Over it, the way the body faces: toward the aim, but no more than 60 degrees off the body's own
|
|
// facing (never folded backwards).
|
|
const Vec3 toward = rotate(about_up(std::clamp(full, -1.05f, 1.05f)), facing);
|
|
// Pointed well up, it is held up to the eye instead: no curl over it, the head tilted back to
|
|
// it (eased in with the tilt).
|
|
const float up_t = std::clamp((pitch - 0.3f) / 0.6f, 0.0f, 1.0f);
|
|
// The back over it as it goes down with the aim, a little at level, more aimed down; the head
|
|
// down to it the same way, a touch at level, more as it goes down; aimed up, the head back.
|
|
const float amount = goal.hunch * std::clamp(0.45f - pitch * (pitch < 0.0f ? 0.55f : 0.7f), 0.2f, 1.2f) * body_weight * (1.0f - up_t);
|
|
const float look = std::clamp(0.2f - pitch * (pitch < 0.0f ? 0.18f : 0.45f), 0.1f, 0.65f) * body_weight * (1.0f - up_t) -
|
|
up_t * std::min(pitch, 1.2f) * 0.45f * body_weight;
|
|
// Up to the eye: the shoulders turn a little, the holding arm's shoulder coming forward, so the
|
|
// arm reaches out in front of the face.
|
|
float shoulders = 0.0f;
|
|
if (up_t > 0.0f) {
|
|
std::array<Placed, arm_reach::right_chain.size()> arm{};
|
|
if (compose(pose, chain(goal.hand), arm)) {
|
|
const Vec3 r = sub(arm[upper].position, arm[chest].position);
|
|
const float way = dot(cross({0.0f, 1.0f, 0.0f}, r), facing) >= 0.0f ? 1.0f : -1.0f;
|
|
shoulders = way * 0.35f * up_t * body_weight;
|
|
}
|
|
}
|
|
// The shoulders and head turn the rest of the way to it off to the side.
|
|
if (!hunch(pose, toward, amount, look, full * (1.0f - hips_share) * body_weight + shoulders)) return {};
|
|
}
|
|
One first;
|
|
first.hand = goal.hand;
|
|
first.front = goal.front;
|
|
first.aim = goal.aim;
|
|
first.front_from_grip = goal.front_from_grip;
|
|
first.worn = goal.worn;
|
|
first.weight = goal.weight;
|
|
const auto held = hold_one(pose, first);
|
|
if (!held.valid || !goal.both) return held;
|
|
// The other hand to its side, underneath, where the first one put it.
|
|
Vec3 support = support_from_grip;
|
|
if (goal.hand == Hand::left) support[0] = -support[0];
|
|
const Vec3 point = add(held.item.position, rotate(held.item.rotation, support));
|
|
One other;
|
|
other.hand = goal.hand == Hand::left ? Hand::right : Hand::left;
|
|
other.aim = normalised(goal.aim);
|
|
other.weight = goal.weight;
|
|
other.free = true;
|
|
other.front = point; // (front_from_grip 0: its grip goes to `point`)
|
|
(void)hold_one(pose, other);
|
|
return held;
|
|
}
|
|
|
|
float pushing(std::span<const Joint> pose) noexcept {
|
|
std::array<Placed, 6> r{}, l{};
|
|
if (!compose(pose, right_leg, r) || !compose(pose, left_leg, l)) return 0.0f;
|
|
const float apart = std::abs(r[5].position[1] - l[5].position[1]);
|
|
return std::clamp((apart - 0.04f) / 0.06f, 0.0f, 1.0f);
|
|
}
|
|
|
|
Vec3 body_facing(std::span<const Joint> pose) noexcept {
|
|
std::array<Placed, 6> right_thigh{}, left_thigh{};
|
|
if (!compose(pose, right_leg, right_thigh) || !compose(pose, left_leg, left_thigh)) return {0, 0, 1};
|
|
Vec3 across = sub(left_thigh[3].position, right_thigh[3].position);
|
|
across[1] = 0.0f;
|
|
return unit(cross(across, {0.0f, 1.0f, 0.0f}), {0, 0, 1});
|
|
}
|
|
}
|