/* * The body as a skeleton: standing, sitting, lying and everything in * between, solved with two-bone IK and kept anatomically sound. */ #include "people/body_internal.h" static Vec3 rotateAbout(Vec3 v, Vec3 axis, float angle); static Vec3 nearestOnSegment(Vec3 a, Vec3 b, Vec3 p); /* ---------- Skeleton ---------- */ /* * Two-bone IK: places mid and end so that root-mid-end has the given * lengths, end lies as close to target as possible and mid bends * towards the pole direction. */ #define FOLDED 0.3f /* a limb shorter than this much of its reach is folded right up ... */ #define FOLD_BAND 0.12f /* ... and this much more: on its way there (blended) */ static void twoBone(Vec3 root, Vec3 target, float l1, float l2, Vec3 pole, Vec3 *mid, Vec3 *end, bool arm) { Vec3 to = vec3_sub(target, root); /* Soft near full stretch: the limb straightens smoothly instead of snapping straight (and back) when the target is just out of reach */ const float full = l1 + l2, soft = full * 0.955f; float want = vec3_length(to); if (want > soft) want = soft + (full - soft) * (1.0f - expf(-(want - soft) / (full - soft))); float dist = clampf(want, fabsf(l1 - l2) + 1e-3f, full - 1e-4f); Vec3 dir = vec3_length(to) > 1e-5f ? vec3_normalize(to) : vec3(0, -1, 0); float along = (l1 * l1 + dist * dist - l2 * l2) / (2.0f * dist); float side = sqrtf(fmaxf(l1 * l1 - along * along, 0.0f)); Vec3 bend = vec3_sub(pole, vec3_scale(dir, vec3_dot(pole, dir))); /* The pole nearly along the limb (a hand right above the shoulder, going round behind the head): its sideways part is tiny and swings wildly - lean on "out to the side" instead */ float clear = vec3_length(bend) / fmaxf(vec3_length(pole), 1e-5f); if (arm && clear < 0.35f) { /* (out to the side, or down and back - whichever is further off the limb, blended) */ Vec3 outward = vec3(root.x >= 0.0f ? 1.0f : -1.0f, -0.2f, -0.3f), downback = vec3(0.0f, -1.0f, -0.5f); Vec3 a1 = vec3_sub(outward, vec3_scale(dir, vec3_dot(outward, dir))); Vec3 a2 = vec3_sub(downback, vec3_scale(dir, vec3_dot(downback, dir))); float w1 = vec3_dot(a1, a1), w2 = vec3_dot(a2, a2); Vec3 alt = vec3_add(vec3_scale(a1, w1 * w1), vec3_scale(a2, w2 * w2)); if (vec3_length(alt) > 1e-4f) { float keep = smoothstepf(0.05f, 0.35f, clear); bend = vec3_add(vec3_scale(vec3_length(bend) > 1e-5f ? vec3_normalize(bend) : alt, keep), vec3_scale(vec3_normalize(alt), 1.0f - keep)); } } bend = vec3_length(bend) > 1e-5f ? vec3_normalize(bend) : vec3(0, 0, 1); *mid = vec3_add(root, vec3_add(vec3_scale(dir, along), vec3_scale(bend, side))); *end = vec3_add(root, vec3_scale(dir, dist)); } /* Bones back to their length along the way they point now (no fitting: for a limb folded right up, where fitting would flip the knee or elbow). */ static Vec3 keepLengths(Vec3 *j, int root, float l1, float l2) { Vec3 a = vec3_sub(j[root + 1], j[root]), b = vec3_sub(j[root + 2], j[root + 1]); if (vec3_length(a) > 1e-4f) j[root + 1] = vec3_add(j[root], vec3_scale(a, l1 / vec3_length(a))); if (vec3_length(b) > 1e-4f) j[root + 2] = vec3_add(j[root + 1], vec3_scale(b, l2 / vec3_length(b))); return j[root + 2]; } /* Puts a limb's bones back to their lengths, its end where it is: stretched out it's solved again (the joint the way the pole says); folded right up (end almost at the root: which way the joint points is anybody's guess, solving would flip it) it keeps its bend. In between the two blend, so going from one to the other never jumps. Returns where the end is. */ static Vec3 refitLimb(Vec3 *j, int r, float l1, float l2, Vec3 pole) { const Vec3 root = j[r], end_was = j[r + 2]; float w = smoothstepf(FOLDED, FOLDED + FOLD_BAND, vec3_length(vec3_sub(end_was, root)) / (l1 + l2)); Vec3 kept_end = keepLengths(j, r, l1, l2), kept_mid = j[r + 1]; if (w <= 0.0f) return kept_end; Vec3 mid, end; twoBone(root, end_was, l1, l2, pole, &mid, &end, false); if (w >= 1.0f) { j[r + 1] = mid; j[r + 2] = end; return end; } Vec3 m = vec3_sub(vec3_lerp(kept_mid, mid, w), root); j[r + 1] = vec3_length(m) > 1e-5f ? vec3_add(root, vec3_scale(m, l1 / vec3_length(m))) : mid; Vec3 e = vec3_sub(vec3_lerp(kept_end, end, w), j[r + 1]); j[r + 2] = vec3_length(e) > 1e-5f ? vec3_add(j[r + 1], vec3_scale(e, l2 / vec3_length(e))) : end; return j[r + 2]; } /* Shoulder -> elbow -> wrist -> hand reaching for a palm position. */ #define WRIST_BEND 0.3f /* the hand goes on along the forearm, bent only this much towards where it reaches (no broken wrists) */ static void reachArm(Vec3 *j, int shoulder, Vec3 palm, Vec3 pole) { Vec3 reach = vec3_normalize(vec3_sub(palm, j[shoulder])); Vec3 dir = reach; for (int pass = 0; pass < 2; pass++) { /* (the second time with the hand along the forearm the first one gave) */ Vec3 wrist_goal = vec3_sub(palm, vec3_scale(dir, HAND)); twoBone(j[shoulder], wrist_goal, UPPER_ARM, FOREARM, pole, &j[shoulder + 1], &j[shoulder + 2], true); Vec3 fore = vec3_sub(j[shoulder + 2], j[shoulder + 1]); if (vec3_length(fore) < 1e-4f) break; dir = vec3_normalize(vec3_lerp(vec3_normalize(fore), reach, WRIST_BEND)); } j[shoulder + 3] = vec3_add(j[shoulder + 2], vec3_scale(vec3_normalize(vec3_sub(palm, j[shoulder + 2])), HAND)); } /* Which way the elbow points reaching for palm: out, down and back; raised over the head, out and a little forward (it never folds in front of the face). */ static Vec3 reachPole(Vec3 shoulder, Vec3 palm, float sign) { Vec3 d = vec3_sub(palm, shoulder); float len = vec3_length(d); float up = len > 1e-4f ? smoothstepf(0.0f, 0.9f, d.y / len) : 0.0f; float across = len > 1e-4f ? smoothstepf(0.0f, 0.8f, -sign * d.x / len) : 0.0f; /* over to the other side: elbow down in front */ return vec3(sign * (0.6f + 0.3f * up) * (1.0f - 0.5f * across), -0.5f * (1.0f - up), -0.5f + 0.75f * up + 0.7f * across); } /* Damped spring step towards a target. */ #define BELLY_PITCH 1.0f /* rad the body bends forward lying over an edge on the belly */ #define SPRING_STEP 0.02f /* s: a slow frame is split into steps this short (60 fps: one) (a stiff spring blew up at 10 fps: NaN arms, a crash) */ static void spring(float *value, float *velocity, float target, float stiffness, float damping, float dt) { int n = dt > SPRING_STEP ? (int)ceilf(dt / SPRING_STEP) : 1; float h = dt / (float)n; for (int i = 0; i < n; i++) { *velocity += (stiffness * (target - *value) - damping * *velocity) * h; *value += *velocity * h; } if (!isfinite(*value) || !isfinite(*velocity)) { *value = target; *velocity = 0.0f; } /* (never a NaN into the body: it would vanish, then crash) */ } /* Head frame from yaw (> 0 = right), pitch (> 0 = up) and roll. */ static void headFrame(float yaw, float pitch, float roll, Vec3 *forward, Vec3 *up) { Vec3 f = vec3(sinf(yaw) * cosf(pitch), sinf(pitch), cosf(yaw) * cosf(pitch)); Vec3 u = vec3(-sinf(yaw) * sinf(pitch), cosf(pitch), -cosf(yaw) * sinf(pitch)); Vec3 r = vec3_cross(u, f); /* figure space: x right, y up, z forward */ *forward = f; *up = vec3_normalize(vec3_add(vec3_scale(u, cosf(roll)), vec3_scale(r, sinf(roll)))); } #define LEG_REACH ((THIGH + SHIN) * 0.975f) /* Highest pelvis that still lets a leg reach its ankle (unscaled figure space). */ static float pelvisReach(Vec3 ankle, float hip_x) { float dx = ankle.x - hip_x, dz = ankle.z; float horizontal = sqrtf(dx * dx + dz * dz); return ankle.y + sqrtf(fmaxf(LEG_REACH * LEG_REACH - horizontal * horizontal, 0.0f)) + HIP_DROP; } /* Ankle to toe tip of a flat foot: feet never get longer than this. */ static float footSpan(void) { return sqrtf(FOOT_LENGTH * FOOT_LENGTH + (ANKLE_Y - 0.02f) * (ANKLE_Y - 0.02f)); } /* Planted foot: when the heel rises the ankle rolls forward over the toes instead of the foot stretching. */ static Vec3 ankleOverToes(Vec3 ankle, Vec3 toe) { const float span = footSpan(); float dy = fminf(ankle.y - toe.y, span * 0.95f); Vec3 flat_d = vec3(ankle.x - toe.x, 0.0f, ankle.z - toe.z); float len = vec3_length(flat_d); if (len < 1e-5f) return ankle; float want = sqrtf(fmaxf(span * span - dy * dy, 0.0f)); return vec3(toe.x + flat_d.x / len * want, toe.y + dy, toe.z + flat_d.z / len * want); } /* A foot behind the body may roll onto its toes: the heel lifts as far as needed (up to HEEL_LIFT) for the leg to reach. Keeps the hips from dropping. */ static Vec3 rollOntoToes(Vec3 ankle, Vec3 hip, float allowed) { float dx = ankle.x - hip.x, dz = ankle.z - hip.z; float h2 = LEG_REACH * LEG_REACH - (dx * dx + dz * dz); if (h2 <= 0.0f) return vec3(ankle.x, ankle.y + allowed, ankle.z); float needed = hip.y - sqrtf(h2); ankle.y = clampf(needed, ankle.y, ankle.y + allowed); return ankle; } /* Burp: a breath in (chest up, chin up), then out with the mouth open. */ static float burpIn(const Body *b) { if (b->burp <= 0.0f) return 0.0f; float p = 1.0f - b->burp / BURP_TIME; return smoothstepf(0.0f, 0.25f, p) * (1.0f - smoothstepf(0.25f, 0.4f, p)); } static float burpOut(const Body *b) { if (b->burp <= 0.0f) return 0.0f; float p = 1.0f - b->burp / BURP_TIME; return smoothstepf(0.3f, 0.4f, p) * (1.0f - smoothstepf(0.7f, 1.0f, p)); } /* ---------- Keeping limbs out of the body ---------- */ /* Pushes p (a limb part of radius r) out of a capsule a-b of radius cr. */ static Vec3 outOfCapsule(Vec3 p, Vec3 a, Vec3 b, float cr, float r, Vec3 fallback) { Vec3 ab = vec3_sub(b, a); float t = clampf(vec3_dot(vec3_sub(p, a), ab) / fmaxf(vec3_dot(ab, ab), 1e-6f), 0.0f, 1.0f); Vec3 c = vec3_add(a, vec3_scale(ab, t)); Vec3 d = vec3_sub(p, c); float len = vec3_length(d), need = cr + r; if (len >= need) return p; Vec3 dir = len > 1e-4f ? vec3_scale(d, 1.0f / len) : fallback; return vec3_add(c, vec3_scale(dir, need)); } /* The torso: belly to chest, flattened front to back (an elliptic cylinder). */ typedef struct { Vec3 a, axis, right, front; float length; } Torso; static Torso torsoOf(const Vec3 *j, Vec3 chest_forward) { Torso t; Vec3 spine = vec3_normalize(vec3_sub(j[JOINT_CHEST], j[JOINT_PELVIS])); t.a = vec3_add(j[JOINT_PELVIS], vec3_scale(spine, 0.05f)); Vec3 b = vec3_add(j[JOINT_CHEST], vec3_scale(spine, 0.03f)); t.length = vec3_length(vec3_sub(b, t.a)); t.axis = vec3_scale(vec3_sub(b, t.a), 1.0f / fmaxf(t.length, 1e-4f)); t.front = vec3_sub(chest_forward, vec3_scale(t.axis, vec3_dot(chest_forward, t.axis))); t.front = vec3_length(t.front) > 1e-4f ? vec3_normalize(t.front) : vec3(0.0f, 0.0f, 1.0f); t.right = vec3_cross(t.axis, t.front); if (t.right.x < 0.0f) t.right = vec3_scale(t.right, -1.0f); /* figure space: +x is right */ return t; } static Vec3 outOfTorso(Vec3 p, const Torso *t, float r, float sign) { float along = vec3_dot(vec3_sub(p, t->a), t->axis); if (along < 0.0f || along > t->length) return p; Vec3 q = vec3_sub(vec3_sub(p, t->a), vec3_scale(t->axis, along)); float x = vec3_dot(q, t->right), z = vec3_dot(q, t->front); float w = TORSO_HALF_WIDTH + r, d = TORSO_HALF_DEPTH + r; float m = sqrtf(x * x / (w * w) + z * z / (d * d)); if (m >= 1.0f) return p; if (m < 1e-3f) { x = sign * w * 0.5f; z = 0.0f; m = 0.5f; } /* deep inside: out to its own side */ return vec3_add(p, vec3_add(vec3_scale(t->right, x / m - x), vec3_scale(t->front, z / m - z))); } /* A point of the arm on side sign, outside torso, hips, thighs and head. */ static Vec3 outOfBody(Vec3 p, const Vec3 *j, const Torso *t, float r, float sign) { Vec3 out = vec3(sign, 0.0f, 0.0f); p = outOfTorso(p, t, r, sign); p = outOfCapsule(p, j[JOINT_HIP_L], j[JOINT_HIP_R], PELVIS_RADIUS, r, out); p = outOfCapsule(p, j[JOINT_HIP_L], j[JOINT_KNEE_L], THIGH_RADIUS, r, vec3(-1.0f, 0.0f, 0.0f)); p = outOfCapsule(p, j[JOINT_HIP_R], j[JOINT_KNEE_R], THIGH_RADIUS, r, vec3(1.0f, 0.0f, 0.0f)); p = outOfCapsule(p, j[JOINT_HEAD], j[JOINT_HEAD], HEAD_RADIUS, r, out); return p; } /* Elbow, wrist and hand never sink into the body; bones keep their length. */ static void keepArmOut(Vec3 *j, int sh, const Torso *t, float sign) { for (int pass = 0; pass < 2; pass++) { Vec3 elbow = outOfBody(j[sh + 1], j, t, ELBOW_RADIUS, sign); j[sh + 1] = vec3_add(j[sh], vec3_scale(vec3_normalize(vec3_sub(elbow, j[sh])), UPPER_ARM)); Vec3 wrist = vec3_add(j[sh + 1], vec3_scale(vec3_normalize(vec3_sub(j[sh + 2], j[sh + 1])), FOREARM)); wrist = outOfBody(wrist, j, t, WRIST_RADIUS, sign); j[sh + 2] = vec3_add(j[sh + 1], vec3_scale(vec3_normalize(vec3_sub(wrist, j[sh + 1])), FOREARM)); Vec3 hand = vec3_add(j[sh + 2], vec3_scale(vec3_normalize(vec3_sub(j[sh + 3], j[sh + 2])), HAND)); hand = outOfBody(hand, j, t, PALM_RADIUS, sign); j[sh + 3] = vec3_add(j[sh + 2], vec3_scale(vec3_normalize(vec3_sub(hand, j[sh + 2])), HAND)); } } /* Palm and elbow direction for an idle gesture (figure space, unscaled). */ static void gesturePalm(const Body *b, const Vec3 *j, const Skeleton *sk, float sign, Vec3 *palm, Vec3 *pole) { Vec3 head = j[JOINT_HEAD], fwd = sk->head_forward, up = sk->head_up; Vec3 side = vec3_normalize(vec3_cross(up, fwd)); /* the head's right */ if (side.x < 0.0f) side = vec3_scale(side, -1.0f); switch (b->gesture) { case GESTURE_HIP: /* hand on the hip, elbow out */ *palm = vec3_add(j[JOINT_PELVIS], vec3(sign * 0.19f, 0.03f, -0.01f)); *pole = vec3(sign, 0.0f, -0.6f); break; case GESTURE_HEAD: { /* scratching the back of the head */ float scratch = 0.012f * sinf(b->clock * 14.0f); *palm = vec3_add(head, vec3_add(vec3_add(vec3_scale(fwd, -0.08f), vec3_scale(up, 0.07f + scratch)), vec3_scale(side, sign * 0.13f))); /* (just clear of the head) */ *pole = vec3(sign, 0.6f, 0.2f); break; } case GESTURE_CHIN: /* thinking: hand at the chin */ *palm = vec3_add(head, vec3_add(vec3_scale(fwd, 0.14f), vec3_add(vec3_scale(up, -0.10f), vec3_scale(side, sign * 0.02f)))); *pole = vec3(sign * 0.4f, -1.0f, 0.2f); break; case GESTURE_NECK: { /* rubbing the back of the neck, elbow up and forward */ float rub = 0.015f * sinf(b->clock * 9.0f); *palm = vec3_add(head, vec3_add(vec3_scale(fwd, -0.10f), vec3_add(vec3_scale(up, -0.09f), vec3_scale(side, sign * (0.05f + rub))))); *pole = vec3(sign * 0.6f, 0.6f, 0.6f); break; } case GESTURE_NOSE: { /* rubbing the nose with a finger */ float rub = 0.008f * sinf(b->clock * 12.0f); *palm = vec3_add(head, vec3_add(vec3_scale(fwd, 0.15f), vec3_add(vec3_scale(up, -0.03f + rub), vec3_scale(side, sign * 0.01f)))); *pole = vec3(sign * 0.4f, -1.0f, 0.2f); break; } case GESTURE_EAR: /* fingers at the earlobe */ *palm = vec3_add(head, vec3_add(vec3_scale(fwd, 0.01f), vec3_add(vec3_scale(up, -0.03f), vec3_scale(side, sign * 0.12f)))); *pole = vec3(sign, -0.6f, 0.2f); break; case GESTURE_HAIR: { /* from the forehead back over the top, then held there a moment */ float u = b->gesture_len > 0.0f ? 1.0f - b->gesture_time / b->gesture_len : 1.0f; float back = smoothstepf(0.15f, 0.55f, u); *palm = vec3_add(head, vec3_add(vec3_scale(fwd, lerpf(0.11f, -0.05f, back)), vec3_add(vec3_scale(up, lerpf(0.09f, 0.13f, back)), vec3_scale(side, sign * 0.04f)))); *pole = vec3(sign, 0.8f, 0.3f); break; } case GESTURE_THUMB: /* thumb hooked in the belt at the front */ *palm = vec3_add(j[JOINT_PELVIS], vec3(sign * 0.10f, 0.01f, 0.12f)); *pole = vec3(sign * 0.8f, -0.3f, -0.5f); break; case GESTURE_CHEST: /* hand flat on the chest */ *palm = vec3_add(j[JOINT_CHEST], vec3(sign * 0.04f, 0.02f, 0.14f)); *pole = vec3(sign * 0.8f, -0.6f, -0.2f); break; case GESTURE_SHADE: /* hand flat over the brow, looking out */ *palm = vec3_add(head, vec3_add(vec3_scale(fwd, 0.15f), vec3_add(vec3_scale(up, 0.12f), vec3_scale(side, sign * 0.06f)))); *pole = vec3(sign, 0.5f, -0.2f); /* (the elbow up and out to the side: the face stays free) */ break; case GESTURE_WAVE: { /* the hand up beside the head, waving side to side */ float wave = 0.07f * sinf(b->clock * 10.0f); *palm = vec3_add(head, vec3_add(vec3_scale(fwd, 0.06f), vec3_add(vec3_scale(up, 0.10f), vec3_scale(side, sign * (0.30f + wave))))); *pole = vec3(sign, -0.8f, -0.2f); break; } case GESTURE_SHRUG: /* the forearm out to the side, the palm up */ *palm = vec3_add(j[JOINT_CHEST], vec3(sign * 0.34f, -0.20f, 0.20f)); *pole = vec3(sign * 0.3f, -1.0f, -0.4f); break; case GESTURE_POINT: /* the arm out ahead, pointing */ *palm = vec3_add(j[JOINT_CHEST], vec3(sign * 0.12f, 0.12f, 0.58f)); *pole = vec3(sign, -1.0f, 0.0f); break; default: /* hand in the trouser pocket */ *palm = vec3_add(j[JOINT_PELVIS], vec3(sign * 0.15f, -0.13f, 0.05f)); *pole = vec3(sign * 0.6f, -0.2f, -1.0f); break; } } /* Where an arm wants its palm, which way its elbow points, and how much it is between two goals (the hand bows out a little on the way). */ typedef struct { Vec3 shoulder, palm, pole; float arc; float sign; /* -1 left arm, 1 right */ } ArmGoal; /* Turns unit direction a towards unit direction b (t = 0..1) along the sphere; straight opposite ones go round by way of `via`. */ static Vec3 turnTowards(Vec3 a, Vec3 b, float t, Vec3 via) { float c = clampf(vec3_dot(a, b), -1.0f, 1.0f); if (c < -0.95f) { /* (almost) opposite: by way of `via` first */ Vec3 v = vec3_sub(via, vec3_scale(a, vec3_dot(via, a))); v = vec3_length(v) > 1e-3f ? vec3_normalize(v) : vec3(1.0f, 0.0f, 0.0f); return t < 0.5f ? turnTowards(a, v, t * 2.0f, via) : turnTowards(v, b, t * 2.0f - 1.0f, via); } float angle = acosf(c); if (angle < 1e-4f) return b; float sa = sinf(angle); return vec3_normalize(vec3_add(vec3_scale(a, sinf((1.0f - t) * angle) / sa), vec3_scale(b, sinf(t * angle) / sa))); } #define ARM_CLEAR 0.34f /* on its way the hand stays this far from the shoulder (and goes round in front) */ #define ARM_FOLDED 0.2f /* ... and never comes closer to it than this */ /* Blends in another goal for the hand. The palm moves on a curve that keeps clear of the shoulder (out in front of it), never through it; the elbow turns with it, down and back on the way. Smooth in the weight and in both goals, whatever they are. */ /* Smoking: where this hand goes (rolling one, the lighter to it, a drag, holding it up in front) and how much (0: none). Unscaled figure space, like the other goals. */ static float smokeArm(const Body *b, const Vec3 *j, const Skeleton *sk, int side, Vec3 *palm, Vec3 *pole) { if (b->smoke_phase == SMOKE_NONE || b->hand_weight[side] > 0.95f) return 0.0f; const float sign = side == 0 ? -1.0f : 1.0f; Vec3 head = j[JOINT_HEAD], fwd = sk->head_forward, up = sk->head_up; Vec3 hs = vec3_normalize(vec3_cross(up, fwd)); if (hs.x < 0.0f) hs = vec3_scale(hs, -1.0f); Vec3 mouth = vec3_add(head, vec3_add(vec3_scale(fwd, 0.13f), vec3_scale(up, -0.075f))); *pole = vec3(sign * 0.4f, -1.0f, 0.2f); float t = b->smoke_t, w = 0.0f; if (b->smoke_phase == SMOKE_ROLL) { /* both hands in front, the paper between them; licked at the end */ w = smoothstepf(0.0f, 0.5f, t); float lick = smoothstepf(ROLL_CIG_TIME - 1.1f, ROLL_CIG_TIME - 0.7f, t); float roll = 0.008f * sinf(t * 7.0f + (side ? PI_F : 0.0f)) * (1.0f - lick); Vec3 lap = vec3_add(j[JOINT_PELVIS], vec3(sign * 0.035f, 0.2f + roll, 0.27f)); *palm = vec3_lerp(lap, vec3_add(mouth, vec3_scale(hs, sign * 0.035f)), lick); if (lick <= 0.0f) *pole = vec3(sign * 0.8f, -0.5f, -0.3f); return w; } if (b->smoke_phase == SMOKE_LIGHT) { /* in the mouth; the lighter to it, the other hand round the flame */ w = smoothstepf(0.0f, 0.4f, t) * (1.0f - smoothstepf(LIGHT_TIME - 0.3f, LIGHT_TIME, t)); bool lighter = side == 1; *palm = vec3_add(mouth, vec3_add(vec3_scale(fwd, lighter ? 0.07f : 0.06f), vec3_add(vec3_scale(up, lighter ? -0.05f : -0.02f), vec3_scale(hs, sign * 0.035f)))); return w; } if (b->cig_hand != side) return 0.0f; float d = DRAG_TIME - b->drag; /* (into the drag) */ float drag = b->drag > 0.0f ? smoothstepf(0.0f, 0.45f, d) * (1.0f - smoothstepf(DRAG_TIME - 0.4f, DRAG_TIME, d)) : 0.0f; Vec3 held = vec3_add(j[side == 0 ? JOINT_SHOULDER_L : JOINT_SHOULDER_R], vec3(sign * 0.09f, -0.24f, 0.26f)); /* forearm up, hand in front of the chest */ Vec3 lips = vec3_add(mouth, vec3_scale(hs, sign * 0.02f)); w = fmaxf(drag, 0.85f * b->cig_hold); *palm = vec3_lerp(held, lips, clampf(drag / fmaxf(w, 1e-3f), 0.0f, 1.0f)); /* (a drag from wherever it was) */ if (drag < 0.5f) *pole = vec3(sign * 0.6f, -1.0f, -0.3f); return w; } static void blendArm(ArmGoal *arm, Vec3 palm, Vec3 pole, float w) { w = clampf(w, 0.0f, 1.0f); if (w <= 0.0f) return; Vec3 p0 = arm->palm, p2 = palm, mid = vec3_scale(vec3_add(p0, p2), 0.5f); /* the way round: mostly forward, in front of the chest */ Vec3 out = vec3_add(vec3_scale(vec3_sub(mid, arm->shoulder), 0.5f), vec3(0.0f, 0.0f, 0.35f)); float near = vec3_length(vec3_sub(mid, arm->shoulder)); Vec3 control = vec3_add(mid, vec3_scale(vec3_normalize(out), fminf(2.2f * fmaxf(ARM_CLEAR - near, 0.0f), 0.32f))); /* from behind the back to the front (or back): round the side, not through the body */ float z0 = p0.z - arm->shoulder.z, z2 = p2.z - arm->shoulder.z; float around = clampf(-z0 * z2 / 0.006f, 0.0f, 1.0f); /* (grows smoothly the more the way crosses) */ if (around > 0.0f) { const float sgn = arm->sign; control.x = lerpf(control.x, sgn * fmaxf(sgn * control.x, sgn * arm->shoulder.x + 0.24f), around); } float a = (1.0f - w) * (1.0f - w), c = 2.0f * w * (1.0f - w), d = w * w; arm->palm = vec3_add(vec3_add(vec3_scale(p0, a), vec3_scale(control, c)), vec3_scale(p2, d)); /* and never right at the shoulder (the arm folded up completely can't tell where its elbow goes) */ Vec3 rel = vec3_add(vec3_sub(arm->palm, arm->shoulder), vec3(0.0f, -0.03f, 0.06f)); float dist = vec3_length(rel); if (dist < ARM_FOLDED) arm->palm = vec3_add(arm->shoulder, vec3_scale(rel, ARM_FOLDED / fmaxf(dist, 1e-4f))); if (vec3_length(pole) > 1e-4f) { Vec3 p = vec3_add(vec3_lerp(arm->pole, vec3_normalize(pole), w), vec3_scale(vec3(0.0f, -0.6f, -0.3f), c)); if (vec3_length(p) > 1e-4f) arm->pole = vec3_normalize(p); } arm->arc = fmaxf(arm->arc, 4.0f * w * (1.0f - w)); } /* Standing, walking, carrying: the full upright body (unscaled). */ static void solveStanding(Body *b, Vec3 position, Vec3 facing, float s, float dt, Skeleton *out) { Vec3 *j = out->joints; const float shy = shyness(b), proud = proudness(b), tired = fmaxf(-b->shown.energy, 0.0f); const float u = b->unsteadiness; /* (the rate changes with how puffed they are: the phase runs at it - sin(clock x rate) raced through whole breaths whenever the rate changed: the shoulders twitched) */ b->breath_phase = fmodf(b->breath_phase + dt * (1.5f + 0.6f * b->shown.nerves + 3.0f * b->exertion), 2.0f * PI_F); const float breath = sinf(b->breath_phase) * (b->asleep ? 1.6f : 1.0f) * (1.0f + 1.5f * b->exertion); /* Feet in figure space (unscaled) */ Vec3 ankle[2], toe[2]; for (int i = 0; i < 2; i++) { ankle[i] = vec3_scale(Figure_toLocal(position, facing, b->feet[i].ankle), 1.0f / s); toe[i] = vec3_scale(Figure_toLocal(position, facing, b->feet[i].toe), 1.0f / s); } float speed = vec3_length(b->velocity); const float still = 1.0f - smoothstepf(0.1f, 0.5f, speed); /* standing about */ const float ws = b->weight_shift * (1.0f - 0.5f * shy); /* > 0: weight on the right leg */ /* ----- Fidgeting feet: one shaken loose in the air, up on the toes, toes tapping ----- */ const float fp = fidgetPhase(b); const float shake = fidgetWeight(b, FIDGET_SHAKE_LEG), rock = fidgetWeight(b, FIDGET_ROCK); const float tap = fidgetWeight(b, FIDGET_TAP_FOOT), stretch = fidgetWeight(b, FIDGET_STRETCH); float raise[2] = { 0.0f, 0.0f }; /* heels lifted on purpose (the toes stay down) */ bool loose[2] = { false, false }; /* not standing on it */ bool tapping[2] = { false, false }; if (shake > 0.001f && !b->feet[0].stepping && !b->feet[1].stepping) { int k = b->fidget_side; float burst = 0.6f + 0.4f * sinf(fp * PI_F * 3.0f); /* in a couple of bursts */ Vec3 off = vec3(0.025f * burst * sinf(b->clock * 23.0f), 0.09f + 0.02f * burst * sinf(b->clock * 19.0f), 0.06f + 0.025f * burst * sinf(b->clock * 21.0f + 1.0f)); ankle[k] = vec3_add(ankle[k], vec3_scale(off, shake)); toe[k] = vec3_add(toe[k], vec3_scale(vec3_add(off, vec3(0.0f, -0.04f, -0.02f)), shake)); /* toes hang down */ loose[k] = shake > 0.25f; } if (rock > 0.001f) { /* up on the toes and down again, twice */ float up = 0.5f - 0.5f * cosf(fp * 4.0f * PI_F); raise[0] = raise[1] = HEEL_LIFT * 0.9f * up * rock; } if (tap > 0.001f && !b->feet[b->fidget_side].stepping) tapping[b->fidget_side] = true; /* weight on one leg: the other relaxes, its heel comes up a little */ if (still > 0.5f) raise[ws > 0.0f ? 0 : 1] += 0.03f * fabsf(ws) * still; raise[0] += 0.025f * stretch; raise[1] += 0.025f * stretch; for (int i = 0; i < 2; i++) ankle[i].y += raise[i]; /* ----- Pelvis: as high as both legs allow, knees soft, swaying over the standing foot ----- */ float stride = ankle[0].z - ankle[1].z; /* > 0: left foot ahead */ /* Only standing feet hold the hips; a swinging leg just bends or reaches. */ float heel[2]; float goal_y = PELVIS_Y; for (int i = 0; i < 2; i++) { heel[i] = b->feet[i].stepping ? 0.0f : HEEL_LIFT * b->feet[i].roll; /* rear foot: toes */ if (!b->feet[i].stepping && !loose[i] && (!b->airborne || b->on_ladder)) /* (in the air the legs just tuck) */ goal_y = fminf(goal_y, pelvisReach(vec3_add(ankle[i], vec3(0.0f, heel[i], 0.0f)), i == 0 ? -HIP_X : HIP_X)); } const float reach_y = goal_y; /* (as high as the standing legs reach) */ goal_y -= b->crouch * 0.42f; goal_y -= 0.010f + 0.022f * fminf(speed / 3.0f, 1.0f) + 0.03f * u + 0.012f * tired + 0.12f * bentDown(b); { /* Walking: up over the standing leg in the middle of each step, down as the other foot lands */ float walk = smoothstepf(0.25f, 0.9f, speed) * (1.0f - smoothstepf(BOB_FROM, BOB_FULL, speed)) * (b->airborne ? 0.0f : 1.0f); float swing = 0.0f; for (int i = 0; i < 2; i++) if (b->feet[i].stepping) swing = fmaxf(swing, sinf(PI_F * b->feet[i].step)); float bob = WALK_BOB * walk * (2.0f * swing - 1.0f) * (1.2f - 0.4f * b->fitness) * (1.0f + 0.3f * b->style[2]); goal_y = fminf(goal_y + bob, reach_y); } /* Dancing: down into the knees on every beat (stompers deeper, swayers softer) */ const float dance = b->dance * still, beat = b->dance_beat - floorf(b->dance_beat); const float on_beat = 0.5f + 0.5f * cosf(2.0f * PI_F * beat); /* (1 on the beat, 0 between) */ const float two = sinf(PI_F * fmodf(b->dance_beat, 2.0f)); /* (-1..1 over two beats: side to side) */ if (dance > 0.001f) { static const float DEPTH[4] = { 0.05f, 0.025f, 0.045f, 0.075f }; goal_y = fminf(goal_y - dance * (0.015f + DEPTH[b->dance_style & 3] * on_beat), reach_y); } /* The hips follow on a spring: they speed up and slow down, never jolt */ { const float was = b->pelvis_y; springStep(&b->pelvis_y, &b->pelvis_vel, goal_y, 0.0f, PELVIS_OMEGA, dt); b->pelvis_vel = clampf(b->pelvis_vel, -PELVIS_SPEED, PELVIS_SPEED); b->pelvis_y = was + clampf(b->pelvis_y - was, -PELVIS_SPEED * dt, PELVIS_SPEED * dt); } float sway = 0.0f; float hip_y[2] = { 0.0f, 0.0f }; for (int i = 0; i < 2; i++) { const Foot *other = &b->feet[1 - i]; if (b->feet[i].stepping && !other->stepping) { /* standing on the other foot */ sway = clampf(ankle[1 - i].x * 0.3f, -SWAY_MOST, SWAY_MOST) /* (side steps: the feet wide apart, the hips stay near the middle) */ * (1.0f + 0.35f * b->style[3]) * sinf(PI_F * b->feet[i].step); /* (some sway their hips more) */ hip_y[i] = -0.018f * sinf(PI_F * b->feet[i].step); /* the free hip drops a little */ } } /* Idle: weight on one leg, that hip up and out */ sway += 0.045f * ws; hip_y[0] += -0.024f * ws; hip_y[1] += 0.024f * ws; if (u > 0.0f) sway += sinf(b->clock * 1.3f) * 0.05f * u; sway += 0.05f * b->teeter * b->balance; /* (balancing on a log: the hips go with the wobble) */ /* Shaking a leg: all the weight over the other foot */ if (shake > 0.001f) sway = lerpf(sway, ankle[1 - b->fidget_side].x * 0.85f, shake); /* Nobody stands quite still: a slow, slight sway, never the same */ float drift_x = still * (0.016f * sinf(b->clock * 0.83f + 3.0f * b->quirk[0]) + 0.007f * sinf(b->clock * 1.91f + 1.0f)); float drift_z = still * (0.011f * sinf(b->clock * 0.61f + 2.0f * b->quirk[1]) + 0.005f * sinf(b->clock * 1.37f)); sway += drift_x; sway += dance * (b->dance_style == 1 ? 0.04f : 0.022f) * two; /* (dancing: the hips from side to side) */ /* Weight moving from foot to foot on springs: steps beginning and ending never jolt the hips */ springCritical(&b->sway_s, &b->sway_v, sway, SWAY_OMEGA, dt); sway = b->sway_s; for (int i = 0; i < 2; i++) { springCritical(&b->hip_s[i], &b->hip_v[i], hip_y[i], SWAY_OMEGA, dt); hip_y[i] = b->hip_s[i]; } float pitch = b->lean * 0.3f + b->crouch * 0.55f + fminf(speed * 0.035f, 0.22f) + 0.2f * runAmount(speed) - 0.06f * proud + 0.12f * shy + 0.09f * tired + 0.08f * u - 0.08f * burpIn(b) + 1.0f * bentDown(b) + 0.1f * heaving(b) * sinf(b->clock * 20.0f) + clampf(b->grade * 0.35f, -0.1f, 0.2f) /* leaning into a climb */ + drift_z * 1.5f - 0.14f * stretch + 0.05f * rock - 0.08f * fidgetWeight(b, FIDGET_ON_HEAD); /* Stepping over something: leaning forward into it, the lifted foot reaches further */ for (int i = 0; i < 2; i++) if (b->feet[i].stepping && b->feet[i].over > 0.0f) pitch += OVER_LEAN * smoothstepf(0.0f, 0.4f, b->feet[i].over / fmaxf(s, 0.1f)) * sinf(PI_F * b->feet[i].step); /* Jogging and running the body rises and falls with every stride, a wave: down onto the landing leg, up again pushing off. The sporty glide along it; the others bounce harder and nod with it */ float wave = 0.0f; { float sum = 0.0f; int n = 0; bool planted = false; for (int i = 0; i < 2; i++) { if (b->feet[i].stepping) { sum += cosf(2.0f * PI_F * b->feet[i].step); n++; } else planted = true; } /* (running: down while a foot is on the ground - the leg bends like a spring - and up in the flight between, past standing height; jogging, the steps' own wave) */ float run = runAmount(speed); float target = lerpf(n ? sum / (float)n : -1.0f, planted ? -1.0f : BOB_FLIGHT, run); springCritical(&b->bob, &b->bob_v, target, BOB_OMEGA, dt); float go = smoothstepf(BOB_FROM, BOB_FULL, speed) * (b->airborne ? 0.0f : 1.0f); float amp = go * (BOB_FIT + BOB_UNFIT * (1.0f - b->fitness)) * (1.0f + BOB_RUN * run) * (1.0f + 0.35f * b->style[2] + 0.25f * clampf(b->shown.energy, 0.0f, 1.0f)); /* (bouncy or flat: their own, livelier when lively) */ wave = 0.5f * (clampf(b->bob, -1.0f, 1.0f + (BOB_FLIGHT - 1.0f) * run) - 1.0f); /* (0 standing .. -1 down; running a little up in the flight) */ pitch += go * BOB_NOD * (1.0f - b->fitness) * wave; wave *= amp; } Vec3 trail = b->root_ok ? vec3_scale(Figure_toLocal(vec3_zero(), facing, vec3_sub(b->root_vis, flat(position))), 1.0f / s) : vec3_zero(); j[JOINT_PELVIS] = vec3(sway + trail.x, b->pelvis_y + wave, -b->lean * 0.05f - b->crouch * 0.12f + drift_z + trail.z); /* Hips turn a little with the forward leg, shoulders counter-turn */ float hip_turn = clampf(stride * 0.2f, -0.12f, 0.12f); float chest_turn = -0.9f * hip_turn + clampf(b->turn_rate * 0.06f, -0.15f, 0.15f) + b->reach_turn + dance * 0.16f * sinf(PI_F * fmodf(b->dance_beat + 0.5f, 2.0f)); /* (dancing: the shoulders go the other way) */ float swing_yaw = 0.0f; /* (the head goes along a little: below) */ { /* Both hands busy off to one side (a swing, a saw): the shoulders turn that way, the hips a little too; up high (winding up) the back arches, down low (the hit) it bends into it */ float w0 = b->hand_ready[0] ? b->hand_weight[0] : 0.0f, w1 = b->hand_ready[1] ? b->hand_weight[1] : 0.0f; if (w0 + w1 > 0.01f) { Vec3 mid = vec3_scale(vec3_add(vec3_scale(b->hand_local[0], w0), vec3_scale(b->hand_local[1], w1)), 1.0f / (w0 + w1)); float yaw = clampf(atan2f(mid.x, fmaxf(mid.z, 0.12f)), -1.0f, 1.0f), both = fminf(w0, w1); chest_turn += SWING_CHEST * yaw * both; hip_turn = clampf(hip_turn + SWING_HIPS * yaw * both, -0.4f, 0.4f); swing_yaw = yaw * both; float up = clampf((mid.y - b->pelvis_y - 0.45f) / 0.5f, -1.0f, 1.0f); /* (over the shoulders .. at the hips) */ pitch -= SWING_ARCH * up * both; } } /* Belly first over an edge: the upper body lies flat over it */ pitch += BELLY_PITCH * b->belly_over; /* Pulling up onto something: the back bends over the edge as far as it takes to keep both hands on it (the shoulders come over the hands) */ { float want = 0.0f; if (b->pulling_up && b->hand_active[0] && b->hand_active[1]) { Vec3 t[2]; for (int k = 0; k < 2; k++) t[k] = vec3_scale(Figure_toLocal(position, facing, b->hand_target[k]), 1.0f / s); const float reach = (UPPER_ARM + FOREARM + 0.5f * HAND) * 0.9f; float best = 1e9f, most = fmaxf(0.0f, 1.15f - pitch); /* (the back flat over, no further) */ for (float e = 0.0f; e <= most + 1e-4f; e += 0.05f) { float pp = pitch + e; Vec3 sh = vec3_add(vec3_add(j[JOINT_PELVIS], vec3_scale(vec3(0.0f, cosf(pp * 0.6f), sinf(pp * 0.6f)), SPINE * 0.47f)), vec3_scale(vec3(0.0f, cosf(pp * 1.3f), sinf(pp * 1.3f)), SPINE * 0.53f + SHOULDER_UP)); float over = 0.0f; for (int k = 0; k < 2; k++) over = fmaxf(over, vec3_length(vec3_sub(t[k], vec3_add(sh, vec3(k == 0 ? -SHOULDER_X : SHOULDER_X, 0.0f, 0.0f)))) - reach); if (over <= 0.0f) { want = e; break; } if (over < best - 0.005f) { best = over; want = e; } } } springCritical(&b->reach_bend, &b->reach_bend_v, want, POSE_OMEGA, dt); pitch += fmaxf(0.0f, b->reach_bend); } float tilt = -sway * 0.4f + (hip_y[1] - hip_y[0]) * 1.2f /* spine leans against the hip tilt */ - 0.22f * b->teeter * b->balance; /* (balancing: the upper body against the wobble) */ /* A back with a waist: the lower spine bends a little, the upper more, and the upper body lags behind starting, stopping and turning (a spring) */ { Vec3 accel = vec3_scale(vec3_sub(b->velocity, b->last_velocity), 1.0f / fmaxf(dt, 1e-3f)); b->last_velocity = b->velocity; Vec3 a = Figure_toLocal(vec3_zero(), facing, accel); Vec3 target = vec3(clampf(-a.x * 0.010f - b->turn_rate * 0.025f, -0.06f, 0.06f), 0.0f, clampf(-a.z * 0.012f, -0.06f, 0.06f)); b->spine_lag_vel = vec3_add(b->spine_lag_vel, vec3_scale(vec3_sub(vec3_scale(vec3_sub(target, b->spine_lag), 70.0f), vec3_scale(b->spine_lag_vel, 9.0f)), fminf(dt, 0.05f))); b->spine_lag = vec3_add(b->spine_lag, vec3_scale(b->spine_lag_vel, fminf(dt, 0.05f))); } Vec3 lower = vec3_normalize(vec3(tilt * 0.5f, cosf(pitch * 0.6f), sinf(pitch * 0.6f))); Vec3 spine = vec3_normalize(vec3(tilt * 1.4f + b->spine_lag.x * 5.0f, cosf(pitch * 1.3f), sinf(pitch * 1.3f) + b->spine_lag.z * 5.0f)); float lift = 0.004f * breath + 0.012f * burpIn(b) + 0.006f * proud; Vec3 waist = vec3_add(j[JOINT_PELVIS], vec3_scale(lower, SPINE * 0.47f)); j[JOINT_CHEST] = vec3_add(vec3_add(waist, vec3_scale(spine, SPINE * 0.53f)), vec3(0.0f, lift, 0.0f)); out->waist_bend = vec3_sub(waist, vec3_lerp(j[JOINT_PELVIS], j[JOINT_CHEST], 0.45f)); j[JOINT_NECK] = vec3_add(j[JOINT_CHEST], vec3_scale(vec3(0.0f, cosf(pitch * 0.6f), sinf(pitch * 0.6f) + 0.1f * shy), NECK)); out->chest_forward = vec3_normalize(vec3(sinf(chest_turn) * 0.3f, -sinf(pitch), cosf(pitch))); /* ----- Head: looks around, leads turns, carries the mood ----- */ float yaw_goal = b->glance_yaw, pitch_goal = b->glance_pitch; if (b->look_weight > 0.0f) { Vec3 l = Figure_toLocal(vec3_zero(), facing, b->look_dir); float yaw = clampf(atan2f(l.x, l.z), -1.1f, 1.1f); float up = clampf(asinf(clampf(l.y / fmaxf(vec3_length(l), 1e-4f), -1.0f, 1.0f)), -0.6f, 0.6f); yaw_goal = yaw_goal + (yaw - yaw_goal) * b->look_weight; pitch_goal = pitch_goal + (up - pitch_goal) * b->look_weight; } yaw_goal += clampf(b->turn_rate * 0.3f, -0.5f, 0.5f); yaw_goal += SWING_HEAD * swing_yaw; /* (with the swing, a little: the eyes stay on the work) */ /* The head stays fairly level while the back leans, except when bent over being sick */ float level = 0.4f * (pitch - 1.0f * bentDown(b)); pitch_goal += 0.12f * proud - 0.28f * shy - 0.1f * tired - 0.15f * u - level + b->chin + 0.25f * burpIn(b) - 0.12f * burpOut(b) - 0.5f * bentDown(b) - 0.35f * peeing(b); for (int i = 0; i < 2; i++) if (b->feet[i].stepping) pitch_goal -= 0.025f * sinf(2.0f * PI_F * b->feet[i].step); /* nod with the steps */ float neck = fidgetWeight(b, FIDGET_NECK); /* stretching the neck: over to one side, then the other */ float roll_goal = -sway * 1.2f + 0.07f * shy + 0.14f * u * sinf(b->clock * 0.9f) + 0.05f * b->quirk[2] + 0.42f * neck * sinf(fp * 2.0f * PI_F) * (b->fidget_side ? 1.0f : -1.0f); pitch_goal += 0.25f * stretch + 0.15f * neck * sinf(fp * 4.0f * PI_F) + still * 0.03f * sinf(b->clock * 0.53f + b->quirk[2]); b->head_yaw = ease(b->head_yaw, yaw_goal, 7.0f, dt); b->head_pitch = ease(b->head_pitch, pitch_goal, 6.0f, dt); b->head_roll = ease(b->head_roll, roll_goal, 4.0f, dt); headFrame(b->head_yaw, b->head_pitch, b->head_roll, &out->head_forward, &out->head_up); j[JOINT_HEAD] = vec3_add(j[JOINT_NECK], vec3_add(vec3_scale(out->head_up, HEAD_UP), vec3_scale(out->head_forward, 0.015f))); /* ----- Legs: IK from the hips to the planted / stepping feet ----- */ /* (pulling up: knees up and out - eased in and out, so they don't swing over when it ends) */ b->pull_knees += clampf((b->pulling_up ? 1.0f : 0.0f) - b->pull_knees, -3.0f * dt, 3.0f * dt); for (int side = 0; side < 2; side++) { float sign = side == 0 ? -1.0f : 1.0f; int hip = side == 0 ? JOINT_HIP_L : JOINT_HIP_R; j[hip] = vec3_add(j[JOINT_PELVIS], vec3(sign * HIP_X * cosf(hip_turn), -HIP_DROP + hip_y[side], -sign * HIP_X * sinf(hip_turn))); Vec3 target = rollOntoToes(ankle[side], j[hip], heel[side]); Vec3 tip = toe[side]; if (b->feet[side].stepping || loose[side] || b->airborne) { /* swinging (or shaken, or in the air): the toes follow the ankle */ tip = vec3_add(target, vec3_scale(vec3_normalize(vec3_sub(tip, target)), footSpan())); } else if (tapping[side]) { /* the heel stays down, the toes come up and tap */ float up = 0.38f * tap * fmaxf(0.0f, sinf(b->clock * 9.0f)); Vec3 v = vec3_sub(tip, target), axis = vec3_cross(v, vec3(0.0f, 1.0f, 0.0f)); if (vec3_length(axis) > 1e-4f) tip = vec3_add(target, rotateAbout(v, vec3_normalize(axis), up)); } else { /* standing: the ankle rolls over the toes */ target = ankleOverToes(target, tip); float slap = b->feet[side].slap; /* just landed on the heel: toes on their way down */ Vec3 v = vec3_sub(tip, target), axis = vec3_cross(v, vec3(0.0f, 1.0f, 0.0f)); if (fabsf(slap) > 1e-3f && vec3_length(axis) > 1e-4f) tip = vec3_add(target, rotateAbout(v, vec3_normalize(axis), slap)); } /* The knee points the way the foot does (the shin can't twist much); shy: knees in a little */ Vec3 fd = vec3(tip.x - target.x, 0.0f, tip.z - target.z); fd = vec3_length(fd) > 0.02f ? vec3_normalize(fd) : vec3(0.0f, 0.0f, 1.0f); Vec3 pole = vec3_add(vec3_scale(fd, KNEE_FOLLOWS_FOOT), vec3(sign * (KNEE_OUT - 0.3f * shy), 0.0f, 1.0f - KNEE_FOLLOWS_FOOT + 0.15f)); /* (straight ahead mostly: toes out don't make bow legs) */ pole.y += 0.8f * b->pull_knees; pole.x += sign * 0.35f * b->pull_knees; /* (getting up onto something: the knee comes up, not into it - and not into the other) */ twoBone(j[hip], target, THIGH, SHIN, pole, &j[hip + 1], &j[hip + 2], false); j[hip + 3] = vec3_add(j[hip + 2], vec3_sub(tip, target)); /* foot keeps its shape if the leg can't reach */ } /* Knees don't pass through each other (shy knees in, crossing steps) */ float knees = j[JOINT_KNEE_R].x - j[JOINT_KNEE_L].x; if (knees < KNEE_GAP) { float push = 0.5f * (KNEE_GAP - knees); j[JOINT_KNEE_L].x -= push; j[JOINT_KNEE_R].x += push; } /* ----- Arms: swing opposite to the leg on the same side, or reach ----- */ /* (standing about, eased: a bump or a short stop doesn't flick the hands into a habit) */ springCritical(&b->idle_s, &b->idle_v, 1.0f - smoothstepf(0.15f, 0.6f, speed), 4.0f, dt); float idle = clampf(b->idle_s, 0.0f, 1.0f); float clasp = idle * smoothstepf(0.3f, 0.7f, shy); /* shy: hands together in front */ float behind = idle * smoothstepf(0.4f, 0.8f, proud); /* proud: hands behind the back */ springCritical(&b->flail, &b->flail_v, fminf(b->stumble / (0.6f * STUMBLE_TIME), 1.0f), 16.0f, dt); const float flail = clampf(b->flail, 0.0f, 1.2f); /* arms out for balance (they fly out, not snap) */ for (int side = 0; side < 2; side++) { float sign = side == 0 ? -1.0f : 1.0f; int sh = side == 0 ? JOINT_SHOULDER_L : JOINT_SHOULDER_R; Vec3 across = vec3(sign * SHOULDER_X * (1.0f - 0.08f * shy) * cosf(chest_turn), 0.0f, -sign * SHOULDER_X * sinf(chest_turn)); Vec3 posture = vec3(0.0f, 0.003f * breath + 0.012f * shy + 0.012f * burpIn(b) - 0.012f * u - 0.01f * tired + 0.01f * sign * b->quirk[1], /* one shoulder a bit higher */ -0.03f * proud + 0.035f * shy + 0.015f * tired); /* Rolling the shoulders: up, back and down, twice; stretching: up a little */ float rolls = fidgetWeight(b, FIDGET_SHOULDERS), ang = fp * 4.0f * PI_F; posture = vec3_add(posture, vec3(0.0f, (0.022f + 0.022f * sinf(ang)) * rolls + 0.02f * stretch, -0.025f * cosf(ang) * rolls)); j[sh] = vec3_add(vec3_add(j[JOINT_CHEST], across), vec3_add(vec3_scale(spine, SHOULDER_UP), posture)); /* Upper arm and forearm follow their swing with a little lag: the hand moves on a rounded path instead of a pendulum */ /* (against the other foot, not the hips: running, both feet are mostly behind the hips - measured from there both arms stayed forward, stiff in front of the chest) */ float foot_ahead = lerpf(ankle[side].z - j[JOINT_PELVIS].z, 0.5f * (ankle[side].z - ankle[1 - side].z), smoothstepf(0.5f, 2.5f, speed)); float run = runAmount(speed); float amplitude = ARM_SWING * (1.0f + 0.25f * b->shown.energy + 0.2f * proud - 0.3f * shy) * (1.0f + 1.2f * run) * (1.0f + 0.2f * sign * b->quirk[0]); /* one arm swings more */ float reach = 0.55f + 0.3f * run; float goal = clampf(-foot_ahead * amplitude, -reach, reach) + flail * 0.5f - 1.5f * drift_z; /* (standing: the arms hang on as the body sways) */ /* Running pumps faster: stiffer springs so the arms keep up with the legs */ float quick = 1.0f + 3.0f * run; float lag = 1.0f - 0.15f * sign * b->quirk[0]; /* ... and a little later */ spring(&b->arm[side], &b->arm_vel[side], goal, ARM_STIFFNESS * quick * lag, ARM_DAMPING * sqrtf(quick * lag), dt); float a = b->arm[side]; /* (walking: a little bend, more swinging forward; running: bent about square, closing as the arm comes forward - the hand up by the chest - and opening on the way back past the hip) */ float walk_bend = 0.25f + clampf(speed * 0.08f, 0.0f, 0.35f) + 0.4f * fmaxf(a, 0.0f); /* (the elbow bends more coming forward) */ float run_bend = 1.4f + 0.35f * a; /* (pumping: about square all the way, the hand up by the chin in front) */ float bend_goal = lerpf(walk_bend, run_bend, run) + 0.15f * shy + 0.08f * sign * b->quirk[3]; spring(&b->fore[side], &b->fore_vel[side], bend_goal, FORE_STIFFNESS * quick, FORE_DAMPING * 0.8f * sqrtf(quick), dt); /* (a little loose: no robot arms) */ float bend = b->fore[side]; Vec3 elbow = swing(j[sh], UPPER_ARM, a, sign * (0.1f - 0.04f * run + 0.04f * proud + 1.4f * flail - 0.07f * fmaxf(a, 0.0f) + 0.03f * b->quirk[3])); Vec3 wrist = swing(elbow, FOREARM, a + bend, sign * (0.03f - 0.16f * fmaxf(a, 0.0f))); Vec3 hand = swing(wrist, HAND, a + bend, 0.0f); /* The swing gives a palm position and the way the elbow points. Habits, gestures and actions blend in their own palm and elbow, and the arm is solved once from the blend: it moves smoothly from one to the next, the elbow never flips, the bones keep their length. */ ArmGoal arm = { j[sh], hand, vec3_sub(elbow, nearestOnSegment(j[sh], hand, elbow)), 0.0f, sign }; if (vec3_length(arm.pole) < 1e-3f) arm.pole = vec3(sign * 0.3f, -0.2f, -1.0f); arm.pole = vec3_normalize(arm.pole); /* Idle habits, hands on the knees while being sick, at the belt while peeing */ float bent = bentDown(b), pee = peeing(b); float gesturing = fmaxf(gestureWeight(b, 0), gestureWeight(b, 1)); float habit = fmaxf(fmaxf(clasp, behind) * (1.0f - gesturing), fmaxf(bent, pee)) * (1.0f - b->hand_weight[side]); if (habit > 0.001f) { /* One hand holds the other's wrist, not a mirror image */ float hold = sign * b->quirk[1] > 0.0f ? 1.0f : 0.0f; Vec3 palm = clasp >= behind ? vec3(sign * (0.035f - 0.03f * hold), 0.86f - 0.025f * hold, 0.17f + 0.01f * hold) : vec3(sign * (0.045f - 0.035f * hold), 0.92f - 0.03f * hold, -0.17f); Vec3 pole = clasp >= behind ? vec3(sign * 0.6f, -0.3f, -0.4f) : vec3(sign * 0.8f, -0.2f, -0.6f); palm = vec3_add(palm, vec3(j[JOINT_PELVIS].x, 0.0f, 0.0f)); if (bent >= fmaxf(clasp, behind)) { /* on the thigh just above the knee (it goes with the knee) */ int hip = side == 0 ? JOINT_HIP_L : JOINT_HIP_R; palm = vec3_add(vec3_lerp(j[hip + 1], j[hip], 0.15f), vec3(sign * 0.03f, 0.06f, 0.03f)); pole = vec3(sign * 0.8f, 0.0f, -0.3f); } if (pee >= fmaxf(fmaxf(clasp, behind), bent)) { palm = vec3(sign * 0.04f + j[JOINT_PELVIS].x, 0.84f, 0.14f); pole = vec3(sign * 0.8f, -0.2f, -0.4f); if (hold > 0.5f && !b->squat_pee) { /* standing: one hand holds it (the Urinella to the body), the other the belt or the hem */ palm = vec3_add(j[JOINT_PELVIS], b->urinella_use ? vec3(sign * 0.02f, -0.12f, 0.12f) : vec3(sign * 0.015f, -0.095f, 0.13f)); pole = vec3(sign * 0.9f, -0.1f, -0.5f); } } blendArm(&arm, palm, pole, habit); } /* An idle gesture with this hand */ float g = gestureWeight(b, side) * (1.0f - b->hand_weight[side]) * (1.0f - fmaxf(bent, pee)); if (g > 0.001f) { Vec3 palm, pole; gesturePalm(b, j, out, sign, &palm, &pole); blendArm(&arm, palm, pole, g); } /* Balancing: the arms out to the sides, the one on the side it tips to lower, the other up */ float bal = b->balance * (1.0f - b->hand_weight[side]); if (bal > 0.001f) blendArm(&arm, vec3(sign * 0.64f, PELVIS_Y + SPINE + 0.06f - 0.16f * b->teeter * sign, 0.06f), vec3(0.0f, -1.0f, -0.5f), bal); /* Fidgets with the arms: stretched up over the head, or folded */ if (stretch > 0.001f) blendArm(&arm, vec3(sign * 0.17f, PELVIS_Y + SPINE + NECK + 0.62f, 0.04f), vec3(sign, 0.1f, -0.3f), stretch); float folded = fidgetWeight(b, FIDGET_CROSS_ARMS); if (folded > 0.001f) { /* each hand at the other upper arm, one forearm over the other */ float over = sign * (b->fidget_side ? 1.0f : -1.0f) > 0.0f ? 1.0f : 0.0f; Vec3 palm = vec3(-sign * 0.15f, 1.13f + 0.05f * over, 0.11f + 0.035f * over); blendArm(&arm, palm, vec3(sign, -0.45f, 0.1f), folded); } /* Two-handed: rubbing them, behind the head, on the hips */ float rubbing = fidgetWeight(b, FIDGET_RUB_HANDS); if (rubbing > 0.001f) { /* palm to palm in front, one sliding up as the other goes down */ float rub = 0.018f * sinf(b->clock * 12.0f + (side ? PI_F : 0.0f)); blendArm(&arm, vec3(sign * 0.025f + j[JOINT_PELVIS].x, 1.0f + rub, 0.25f), vec3(sign * 0.8f, -0.5f, -0.3f), rubbing); } float behind_head = fidgetWeight(b, FIDGET_HEAD_BEHIND); if (behind_head > 0.001f) blendArm(&arm, vec3_add(j[JOINT_HEAD], vec3(sign * 0.05f, 0.02f, -0.11f)), vec3(sign, 0.5f, -0.1f), behind_head); float on_head = fidgetWeight(b, FIDGET_ON_HEAD); if (on_head > 0.001f) /* (fingers laced on top of the head, elbows out and up) */ blendArm(&arm, vec3_add(j[JOINT_HEAD], vec3(sign * 0.035f, 0.13f, 0.0f)), vec3(sign, 0.7f, 0.1f), on_head); float akimbo = fidgetWeight(b, FIDGET_AKIMBO); if (akimbo > 0.001f) blendArm(&arm, vec3_add(j[JOINT_PELVIS], vec3(sign * 0.19f, 0.03f, -0.01f)), vec3(sign, 0.0f, -0.6f), akimbo); /* Smoking: rolling, lighting, a drag, holding it */ { Vec3 palm, pole; float smoke = smokeArm(b, j, out, side, &palm, &pole) * (1.0f - b->hand_weight[side]); if (smoke > 0.001f) blendArm(&arm, palm, pole, smoke); } /* Dancing: fists pumping at the chest, swinging low, both up in the air now and then, one fist up on the beat */ float dw = dance * (1.0f - b->hand_weight[side]) * (1.0f - fmaxf(bent, pee)); if (dw > 0.001f) { float bar = fmodf(b->dance_beat, 16.0f), up = smoothstepf(7.5f, 8.5f, bar) * (1.0f - smoothstepf(15.0f, 16.0f, bar)); float pump = 0.07f * (on_beat - 0.5f) * (side ? 1.0f : -1.0f) * (b->dance_style == 0 ? 1.0f : 0.6f); Vec3 palm = vec3(sign * 0.21f, PELVIS_Y + SPINE - 0.22f + pump, 0.24f), pole = vec3(sign, -0.6f, -0.3f); switch (b->dance_style & 3) { case 1: palm = vec3(sign * 0.24f + 0.1f * two, PELVIS_Y + SPINE - 0.36f + 0.03f * on_beat, 0.18f); break; case 2: palm = vec3_lerp(palm, vec3(sign * 0.27f, PELVIS_Y + SPINE + 0.55f + 0.04f * on_beat, 0.08f + 0.04f * two * sign), up); pole = vec3_lerp(pole, vec3(sign, 0.2f, -0.2f), up); break; case 3: if (sign * (b->quirk[0] >= 0.0f ? 1.0f : -1.0f) > 0.0f) { /* (the fist up in the air: on their stronger side) */ palm = vec3(sign * 0.18f, PELVIS_Y + SPINE + 0.3f + 0.16f * on_beat, 0.3f); pole = vec3(sign, 0.0f, -0.4f); } break; default: break; } blendArm(&arm, palm, pole, dw); } /* Actions: reaching, carrying, pushing */ float w = b->hand_weight[side]; Vec3 target = vec3_scale(Figure_toLocal(position, facing, b->hand_target[side]), 1.0f / s); if (!b->hand_ready[side] || w < 0.02f) { /* (not reaching yet: it sets off from where the hand is) */ b->hand_local[side] = arm.palm; b->hand_vel[side] = vec3_zero(); b->hand_ready[side] = true; } else if (dt > 0.0f) { /* The hand goes after its goal on a spring: when the goal jumps, the hand doesn't. A goal moving along (a rung while climbing past it, a swing) is followed without lagging behind. */ Vec3 along = vec3_scale(vec3_sub(target, b->hand_goal_was[side]), 1.0f / dt); if (vec3_length(along) > HAND_FOLLOW) along = vec3_zero(); /* (that's a jump) */ springStep3(&b->hand_local[side], &b->hand_vel[side], target, along, HAND_OMEGA, dt); } b->hand_goal_was[side] = target; if (w > 0.001f) blendArm(&arm, b->hand_local[side], reachPole(j[sh], b->hand_local[side], sign), w); /* Moving between goals the hand takes a little curve out from the body */ Vec3 palm = vec3_add(arm.palm, vec3(sign * 0.06f * arm.arc, 0.0f, 0.04f * arm.arc)); reachArm(j, sh, palm, arm.pole); } /* Anatomy: arms move around the body, never through it */ Torso torso = torsoOf(j, out->chest_forward); keepArmOut(j, JOINT_SHOULDER_L, &torso, -1.0f); keepArmOut(j, JOINT_SHOULDER_R, &torso, 1.0f); } /* Lying on the ground, head along d (unscaled figure space). */ static void solveLying(const Body *b, Vec3 d, float s, Skeleton *out) { Vec3 *j = out->joints; const bool down = b->face_down; const Vec3 body_up = d; /* towards the head */ const Vec3 body_fwd = down ? vec3(0, -1, 0) : UP; /* chest faces the ground or the sky */ const Vec3 body_right = vec3_cross(body_up, body_fwd); const Vec3 anchor = vec3_add(vec3_scale(d, b->lie_offset), vec3(0.0f, LIE_HEIGHT, 0.0f)); const float breath = sinf(b->clock * (b->asleep ? 1.1f : 2.2f)) * 0.006f; /* Relaxed body, described like a standing one: x right, y up from the hips, z front */ #define AT(x, y, z) vec3_add(anchor, vec3_add(vec3_add(vec3_scale(body_right, (x)), \ vec3_scale(body_up, (y) - PELVIS_Y)), vec3_scale(body_fwd, (z)))) j[JOINT_PELVIS] = AT(0.0f, PELVIS_Y, 0.0f); j[JOINT_CHEST] = AT(0.0f, PELVIS_Y + SPINE, breath); j[JOINT_NECK] = AT(0.0f, PELVIS_Y + SPINE + NECK, 0.0f); j[JOINT_HEAD] = AT(0.0f, PELVIS_Y + SPINE + NECK + HEAD_UP, 0.01f); for (int side = 0; side < 2; side++) { float sign = side == 0 ? -1.0f : 1.0f; int hip = side == 0 ? JOINT_HIP_L : JOINT_HIP_R; bool knee_up = !down && b->asleep && side == 0; /* one knee raised in sleep */ j[hip] = AT(sign * HIP_X, PELVIS_Y - HIP_DROP, 0.0f); j[hip + 1] = knee_up ? AT(sign * 0.13f, 0.56f, 0.2f) : AT(sign * 0.12f, 0.49f, 0.02f); j[hip + 2] = knee_up ? AT(sign * 0.14f, 0.14f, 0.0f) : AT(sign * 0.15f, 0.075f, 0.0f); j[hip + 3] = down ? AT(sign * 0.16f, -0.04f, 0.06f) : AT(sign * 0.2f, 0.06f, 0.13f); if (b->rolling && b->roll_over == 0.0f) { /* stretched out like a log: legs together, toes pointed */ j[hip + 1] = AT(sign * 0.08f, 0.49f, 0.0f); j[hip + 2] = AT(sign * 0.065f, 0.075f, 0.0f); j[hip + 3] = AT(sign * 0.065f, -0.06f, -0.03f); } int sh = side == 0 ? JOINT_SHOULDER_L : JOINT_SHOULDER_R; j[sh] = AT(sign * SHOULDER_X, PELVIS_Y + SPINE + SHOULDER_UP, 0.0f); Vec3 palm = down ? AT(sign * 0.27f, PELVIS_Y + SPINE + 0.32f, 0.0f) /* caught the fall by the head */ : AT(sign * 0.5f, PELVIS_Y + 0.0f, 0.03f); /* arms out */ if (b->rolling && b->roll_over == 0.0f) palm = AT(sign * 0.13f, PELVIS_Y + SPINE + 0.68f, 0.02f); /* arms stretched over the head */ else if (b->roll_over != 0.0f) palm = AT(sign * 0.22f, PELVIS_Y + 0.05f, down ? -0.06f : 0.06f); /* (arms in, along the body) */ reachArm(j, sh, palm, down ? vec3_scale(body_right, sign) : vec3_scale(body_up, -1.0f)); } #undef AT /* Rolling: the whole body turns about its length and moves sideways with it */ if (b->rolling && fabsf(b->roll) > 1e-4f) { Vec3 axis = d, aside = vec3_scale(vec3_normalize(vec3_cross(d, UP)), b->roll_side / s); for (int k = 0; k < JOINT_COUNT; k++) j[k] = vec3_add(vec3_add(anchor, rotateAbout(vec3_sub(j[k], anchor), axis, b->roll)), aside); out->chest_forward = rotateAbout(body_fwd, axis, b->roll); out->head_up = body_up; out->head_forward = rotateAbout(down ? body_right : body_fwd, axis, b->roll); for (int k = 0; k < JOINT_COUNT; k++) { Vec3 rel = vec3_sub(j[k], vec3_scale(d, b->lie_offset)); j[k].y += lyingGround(b, vec3_dot(vec3(rel.x, 0.0f, rel.z), d) * s) / s; } return; } /* Follow the ground: every joint sits on the terrain below it */ for (int k = 0; k < JOINT_COUNT; k++) { Vec3 rel = vec3_sub(j[k], vec3_scale(d, b->lie_offset)); float along = vec3_dot(vec3(rel.x, 0.0f, rel.z), d); j[k].y += lyingGround(b, along * s) / s; } out->chest_forward = body_fwd; out->head_up = body_up; /* Face down: head turned to one side to breathe */ out->head_forward = down ? body_right : vec3_normalize(vec3_add(body_fwd, vec3_scale(body_up, 0.15f))); } /* Kneeling on the left knee, hands on the right knee: halfway up. */ static void solveKneeling(Skeleton *out) { Vec3 *j = out->joints; j[JOINT_PELVIS] = vec3(0.0f, 0.55f, -0.05f); j[JOINT_CHEST] = vec3(0.0f, 0.88f, 0.08f); j[JOINT_NECK] = vec3(0.0f, 1.04f, 0.13f); out->head_forward = vec3_normalize(vec3(0.0f, -0.35f, 1.0f)); out->head_up = vec3_normalize(vec3(0.0f, 1.0f, 0.35f)); out->chest_forward = vec3_normalize(vec3(0.0f, -0.4f, 1.0f)); j[JOINT_HEAD] = vec3_add(j[JOINT_NECK], vec3_scale(out->head_up, HEAD_UP)); j[JOINT_HIP_L] = vec3(-HIP_X, 0.50f, -0.05f); j[JOINT_KNEE_L] = vec3(-0.10f, 0.07f, 0.0f); j[JOINT_ANKLE_L] = vec3(-0.10f, 0.09f, -0.42f); j[JOINT_TOE_L] = vec3(-0.10f, 0.02f, -0.53f); j[JOINT_HIP_R] = vec3(HIP_X, 0.50f, -0.05f); j[JOINT_KNEE_R] = vec3(0.11f, 0.48f, 0.37f); j[JOINT_ANKLE_R] = vec3(0.11f, ANKLE_Y, 0.33f); j[JOINT_TOE_R] = vec3(0.11f, 0.02f, 0.48f); for (int side = 0; side < 2; side++) { float sign = side == 0 ? -1.0f : 1.0f; int sh = side == 0 ? JOINT_SHOULDER_L : JOINT_SHOULDER_R; j[sh] = vec3(sign * SHOULDER_X, 0.94f, 0.1f); reachArm(j, sh, vec3(0.06f + sign * 0.05f, 0.55f, 0.4f), vec3(sign, 0.0f, -0.3f)); } } /* Sitting: cross-legged on the ground, or on a seat behind the feet. Unscaled. */ static void solveSitting(Skeleton *out, int style, float seat_height, float breath) { Vec3 *j = out->joints; bool seat = style == SIT_SEAT; float h = seat ? (seat_height > 0.05f ? seat_height : SEAT_HEIGHT) : 0.0f, back = seat ? -0.2f : -0.05f; j[JOINT_PELVIS] = vec3(0.0f, h + 0.16f, back); j[JOINT_CHEST] = vec3(0.0f, h + 0.51f + 0.004f * breath, back + 0.07f); j[JOINT_NECK] = vec3(0.0f, h + 0.67f, back + 0.1f); out->head_forward = vec3_normalize(vec3(0.0f, -0.2f, 1.0f)); out->head_up = vec3_normalize(vec3(0.0f, 1.0f, 0.2f)); out->chest_forward = vec3_normalize(vec3(0.0f, -0.2f, 1.0f)); out->waist_bend = vec3(0.0f, 0.0f, -0.025f); /* a relaxed, rounded back */ j[JOINT_HEAD] = vec3_add(j[JOINT_NECK], vec3_scale(out->head_up, HEAD_UP)); for (int side = 0; side < 2; side++) { float sign = side == 0 ? -1.0f : 1.0f; int hip = side == 0 ? JOINT_HIP_L : JOINT_HIP_R, o = side * 4; j[hip] = vec3(sign * HIP_X, h + 0.11f, back); if (seat) { /* knees forward, feet on the ground */ j[JOINT_KNEE_L + o] = vec3(sign * 0.12f, h + 0.05f, back + 0.43f); j[JOINT_ANKLE_L + o] = vec3(sign * 0.13f, ANKLE_Y, back + 0.45f); j[JOINT_TOE_L + o] = vec3(sign * 0.13f, 0.02f, back + 0.58f); } else { /* knees out, shins crossed in front */ j[JOINT_KNEE_L + o] = vec3(sign * 0.36f, 0.11f, back + 0.22f); j[JOINT_ANKLE_L + o] = vec3(-sign * 0.11f, 0.07f, back + (side ? 0.30f : 0.25f)); j[JOINT_TOE_L + o] = vec3(-sign * 0.24f, 0.04f, back + (side ? 0.27f : 0.22f)); } int sh = side == 0 ? JOINT_SHOULDER_L : JOINT_SHOULDER_R; j[sh] = vec3(sign * SHOULDER_X, j[JOINT_CHEST].y + 0.05f, back + 0.08f); Vec3 knee = j[JOINT_KNEE_L + o]; reachArm(j, sh, vec3(knee.x * 0.8f, knee.y + 0.07f, knee.z - 0.05f), vec3(sign, 0.0f, -0.3f)); } } static void blendSkeleton(Skeleton *a, const Skeleton *b, float t) { for (int i = 0; i < JOINT_COUNT; i++) a->joints[i] = vec3_lerp(a->joints[i], b->joints[i], t); a->waist_bend = vec3_lerp(a->waist_bend, b->waist_bend, t); a->head_forward = vec3_normalize(vec3_lerp(a->head_forward, b->head_forward, t)); a->head_up = vec3_normalize(vec3_lerp(a->head_up, b->head_up, t)); a->chest_forward = vec3_normalize(vec3_lerp(a->chest_forward, b->chest_forward, t)); } /* Rotates a point / direction about axis (unit) through the origin. */ static Vec3 rotateAbout(Vec3 v, Vec3 axis, float angle) { float c = cosf(angle), sn = sinf(angle); return vec3_add(vec3_add(vec3_scale(v, c), vec3_scale(vec3_cross(axis, v), sn)), vec3_scale(axis, vec3_dot(axis, v) * (1.0f - c))); } /* Turns the whole body about the vertical through the feet. */ static void turnAbout(Skeleton *sk, float angle) { if (fabsf(angle) < 1e-4f) return; for (int i = 0; i < JOINT_COUNT; i++) sk->joints[i] = rotateAbout(sk->joints[i], UP, angle); sk->head_forward = rotateAbout(sk->head_forward, UP, angle); sk->head_up = rotateAbout(sk->head_up, UP, angle); sk->chest_forward = rotateAbout(sk->chest_forward, UP, angle); } /* Toppling like a plank around the feet, then landing in the lying pose. */ static void fallOver(Skeleton *sk, Vec3 d, float t) { Vec3 axis = vec3_normalize(vec3_cross(UP, d)); /* turns up towards d */ float angle = (PI_F / 2.0f) * t * t; for (int i = 0; i < JOINT_COUNT; i++) sk->joints[i] = rotateAbout(sk->joints[i], axis, angle); sk->head_forward = rotateAbout(sk->head_forward, axis, angle); sk->head_up = rotateAbout(sk->head_up, axis, angle); sk->chest_forward = rotateAbout(sk->chest_forward, axis, angle); } static Face faceFor(const Body *b) { const float shy = shyness(b), proud = proudness(b), u = b->unsteadiness; const float tired = fmaxf(-b->shown.energy, 0.0f), lively = fmaxf(b->shown.energy, 0.0f); Face face = { .mouth = fmaxf(burpOut(b), heaving(b)), .eyelids = fmaxf(b->blink > 0.0f ? 1.0f : 0.0f, 0.35f * u + 0.25f * tired), .brow = clampf(0.2f * proud + 0.6f * burpIn(b) - 0.8f * shy - 0.2f * u, -1.0f, 1.0f), .smile = clampf(0.35f * proud + 0.2f * lively + 0.25f * u - 0.3f * tired - 0.1f * shy, -1.0f, 1.0f), .gaze = clampf(-0.7f * shy + 0.2f * proud, -1.0f, 1.0f), }; if (b->sulk > 0.0f) { /* annoyed: brows down, mouth too */ face.brow = lerpf(face.brow, -0.6f, 0.8f * b->sulk); face.smile = lerpf(face.smile, -0.6f, 0.8f * b->sulk); } if (b->disgust > 0.0f) { /* nose wrinkled, mouth pulled down */ face.brow = lerpf(face.brow, -0.7f, b->disgust); face.smile = lerpf(face.smile, -0.9f, b->disgust); face.eyelids = fmaxf(face.eyelids, 0.4f * b->disgust); } if (b->pose != POSE_UP) { face.eyelids = b->asleep ? 1.0f : fmaxf(face.eyelids, 0.3f); face.mouth = b->asleep ? 0.2f : 0.0f; } return face; } /* Peeing or being sick: an arc from the body down to the ground (unscaled). */ /* A stream's arc (figure space, unscaled): from start, along dir at speed, falling. */ typedef struct { Vec3 start, dir; float speed, down; } StreamArc; static Vec3 arcAt(const StreamArc *a, float t) { const float GRAVITY = 9.81f; return vec3_add(vec3_add(a->start, vec3_scale(a->dir, a->speed * t)), vec3(0.0f, -a->down * t - 0.5f * GRAVITY * t * t, 0.0f)); } /* The cigarette, from the final pose: between the fingers, in the lips, or the paper between the hands being rolled. */ static void placeCigarette(const Body *b, float s, Skeleton *out) { out->cig_glow = -1.0f; if (b->smoke_phase == SMOKE_NONE || b->pose != POSE_UP) return; const Vec3 *j = out->joints; float len = 0.078f * s * (0.3f + 0.7f * clampf(b->cig_left, 0.0f, 1.0f)); if (b->smoke_phase == SMOKE_ROLL) { Vec3 l = j[JOINT_HAND_L], r = j[JOINT_HAND_R], mid = vec3_scale(vec3_add(l, r), 0.5f); Vec3 across = vec3_sub(r, l); across = vec3_length(across) > 1e-3f ? vec3_normalize(across) : vec3(1.0f, 0.0f, 0.0f); out->cig_a = vec3_sub(mid, vec3_scale(across, 0.035f * s)); out->cig_tip = vec3_add(mid, vec3_scale(across, 0.035f * s)); out->cig_glow = 0.0f; return; } Vec3 fwd = out->head_forward, up = out->head_up; if (b->smoke_phase == SMOKE_LIGHT || b->cig_hand == 2) { /* in the lips, pointing down a little */ out->cig_a = vec3_add(j[JOINT_HEAD], vec3_scale(vec3_add(vec3_scale(fwd, 0.095f), vec3_scale(up, -0.062f)), s)); out->cig_tip = vec3_add(out->cig_a, vec3_scale(vec3_normalize(vec3_add(vec3_scale(fwd, 0.9f), vec3_scale(up, -0.35f))), len)); } else { int side = b->cig_hand; float sign = side == 0 ? -1.0f : 1.0f; Vec3 hand = j[side == 0 ? JOINT_HAND_L : JOINT_HAND_R], wrist = j[side == 0 ? JOINT_WRIST_L : JOINT_WRIST_R]; Vec3 dir = vec3_sub(hand, wrist); dir = vec3_length(dir) > 1e-4f ? vec3_normalize(dir) : vec3(0.0f, -1.0f, 0.0f); Vec3 o = vec3(sign * 0.5f, 0.15f, 0.85f); /* (out of the fingers: forward and away from the body) */ o = vec3_sub(o, vec3_scale(dir, vec3_dot(o, dir))); o = vec3_length(o) > 1e-3f ? vec3_normalize(o) : vec3(0.0f, 0.0f, 1.0f); out->cig_a = vec3_add(hand, vec3_add(vec3_scale(dir, 0.02f * s), vec3_scale(o, -0.012f * s))); out->cig_tip = vec3_add(out->cig_a, vec3_scale(o, len)); } float drag = b->drag > 0.0f ? smoothstepf(0.0f, 0.5f, DRAG_TIME - b->drag) * smoothstepf(0.0f, 0.3f, b->drag) : 0.0f; out->cig_glow = b->smoke_phase == SMOKE_LIGHT ? smoothstepf(LIGHT_TIME * 0.55f, LIGHT_TIME * 0.8f, b->smoke_t) : 0.25f + 0.75f * drag; } static void solveStream(const Body *b, Vec3 facing, Skeleton *out, StreamArc *arc) { const float GRAVITY = 9.81f; out->stream_kind = STREAM_NONE; out->uri = false; /* squatting to pee: trousers down, skirt or dress gathered up; standing (a man, a woman with a Urinella): the fly open, the skirt or dress lifted in front (negative). Eased with the pee. */ out->undress = b->pose != POSE_UP ? 0.0f : b->squat_pee ? smoothstepf(0.0f, 0.5f, peeing(b)) * smoothstepf(0.4f, 0.8f, b->crouch) : -smoothstepf(0.0f, 0.6f, peeing(b)); if (b->pose != POSE_UP) return; const Vec3 *j = out->joints; float pee = peeing(b), heave = heaving(b); Vec3 start, dir; float speed, down; if (pee > 0.05f && b->squat_pee) { /* squatting: straight down */ if (b->crouch < 0.7f) return; start = vec3_add(j[JOINT_PELVIS], vec3(0.0f, -0.1f, 0.02f)); dir = vec3(0.0f, 0.0f, 1.0f); speed = 0.15f * pee; down = 0.6f; out->stream_kind = STREAM_PEE; out->stream_width = 0.008f + 0.004f * pee; } else if (pee > 0.05f) { /* Aimed where they look (within reason); pushing hard goes further */ start = vec3_add(j[JOINT_PELVIS], vec3(0.0f, -0.11f, 0.11f)); Vec3 aim = vec3_length(b->pee_aim) > 0.1f ? Figure_toLocal(vec3_zero(), facing, b->pee_aim) : vec3(0.0f, -0.3f, 1.0f); float yaw = clampf(atan2f(aim.x, aim.z), -0.7f, 0.7f) + 0.04f * sinf(b->clock * 1.7f); /* wavers a little */ float pitch = clampf(atan2f(aim.y, sqrtf(aim.x * aim.x + aim.z * aim.z)), -0.9f, 0.65f); float v = (1.3f + 2.0f * b->pee_power) * pee; dir = vec3(sinf(yaw), 0.0f, cosf(yaw)); speed = v * cosf(pitch); down = -v * sinf(pitch); out->stream_kind = STREAM_PEE; out->stream_width = 0.009f + 0.003f * pee; if (b->urinella_use) { /* held to the body, the spout out in front: the stream from its end */ out->uri = true; out->uri_a = vec3_add(j[JOINT_PELVIS], vec3(0.0f, -0.15f, 0.075f)); out->uri_b = vec3_add(start, vec3_add(vec3_scale(dir, 0.05f), vec3(0.0f, 0.01f, 0.0f))); start = vec3_add(out->uri_b, vec3_scale(dir, 0.01f)); } } else if (heave > 0.2f) { start = vec3_add(j[JOINT_HEAD], vec3_sub(vec3_scale(out->head_forward, 0.09f), vec3_scale(out->head_up, 0.06f))); dir = vec3(out->head_forward.x, 0.0f, out->head_forward.z); speed = 0.8f * heave; down = 0.8f; out->stream_kind = STREAM_VOMIT; out->stream_width = 0.022f + 0.01f * sinf(b->clock * 23.0f); } else { return; } dir = vec3_length(dir) > 1e-3f ? vec3_normalize(dir) : vec3(0.0f, 0.0f, 1.0f); float ground = fminf(j[JOINT_ANKLE_L].y, j[JOINT_ANKLE_R].y) - ANKLE_Y; float drop = start.y - ground; if (drop < 0.05f) { out->stream_kind = STREAM_NONE; return; } float land = (-down + sqrtf(down * down + 2.0f * GRAVITY * drop)) / GRAVITY; *arc = (StreamArc){ start, dir, speed, down }; for (int k = 0; k < 4; k++) out->stream[k] = arcAt(arc, land * (float)k / 3.0f); } /* The stream comes down on the real ground (a slope, a hollow, something standing there), not where the feet are: followed along until it meets it. */ static void landStream(const Body *b, Vec3 position, Vec3 facing, float s, const StreamArc *arc, Skeleton *out) { if (out->stream_kind == STREAM_NONE || !b->ground) return; const float step = 0.01f; float t = 0.0f; for (int k = 0; k < 300; k++, t += step) { Vec3 w = Figure_toWorld(position, facing, vec3_scale(arcAt(arc, t + step), s)); if (w.y <= b->ground(b->clutter_ctx, w.x, w.z, w.y + 0.3f * s)) break; } if (t < step) { out->stream_kind = STREAM_NONE; return; } for (int k = 0; k < 4; k++) out->stream[k] = vec3_scale(arcAt(arc, t * (float)k / 3.0f), s); } /* ---------- Anatomy: whatever was blended, bones keep their length ---------- */ static Vec3 nearestOnSegment(Vec3 a, Vec3 b, Vec3 p) { Vec3 ab = vec3_sub(b, a); float t = clampf(vec3_dot(vec3_sub(p, a), ab) / fmaxf(vec3_dot(ab, ab), 1e-8f), 0.0f, 1.0f); return vec3_add(a, vec3_scale(ab, t)); } /* Moves joint `from` (and everything listed after it) so it sits at least `len` from `base`. */ static void atLeast(Vec3 *j, int base, int from, float len, Vec3 fallback, const int *carry, int n) { Vec3 d = vec3_sub(j[from], j[base]); float l = vec3_length(d); if (l >= len) return; Vec3 dir = l > 1e-4f ? vec3_scale(d, 1.0f / l) : fallback; Vec3 shift = vec3_sub(vec3_add(j[base], vec3_scale(dir, len)), j[from]); j[from] = vec3_add(j[from], shift); for (int i = 0; i < n; i++) j[carry[i]] = vec3_add(j[carry[i]], shift); } /* A limb (root, middle, end, tip) solved again from root to end with the bend it has now, so blends between poses never shrink or stretch the bones. knee_rule: the knee may not bend against the way the foot points. */ static void reLimb(Vec3 *j, int root, float l1, float l2, float l3, Vec3 fallback, bool knee_rule, Vec3 *last, bool *last_ok, float dt) { Vec3 r = j[root], m = j[root + 1], e = j[root + 2], t = j[root + 3]; Vec3 axis = vec3_sub(e, r); axis = vec3_length(axis) > 1e-4f ? vec3_normalize(axis) : vec3(0.0f, -1.0f, 0.0f); Vec3 pole = vec3_sub(m, nearestOnSegment(r, e, m)); float bent = vec3_length(pole); if (bent < 1e-3f) pole = *last_ok ? *last : fallback; /* straight: it keeps bending the way it did */ Vec3 tail = vec3_sub(t, e); if (knee_rule && vec3_length(tail) > 1e-3f && vec3_dot(pole, tail) < 0.0f) pole = vec3_add(pole, vec3_scale(vec3_normalize(tail), 2.0f * vec3_length(pole) + 0.05f)); /* A knee or elbow never flips over to the other side from one frame to the next: the way it points turns round the limb at a limited speed */ Vec3 perp = vec3_sub(pole, vec3_scale(axis, vec3_dot(pole, axis))); if (vec3_length(perp) > 1e-4f) { perp = vec3_normalize(perp); if (*last_ok) { Vec3 was = vec3_sub(*last, vec3_scale(axis, vec3_dot(*last, axis))); if (vec3_length(was) > 1e-3f) { was = vec3_normalize(was); float angle = acosf(clampf(vec3_dot(was, perp), -1.0f, 1.0f)), most = POLE_TURN * dt; if (angle > most) perp = turnTowards(was, perp, most / angle, vec3_cross(axis, was)); } } pole = perp; *last = pole; *last_ok = true; } Vec3 m2, e2; twoBone(r, e, l1, l2, pole, &m2, &e2, false); j[root + 1] = m2; j[root + 2] = e2; float tl = vec3_length(tail); j[root + 3] = vec3_add(e2, vec3_scale(tl > 1e-4f ? vec3_scale(tail, 1.0f / tl) : fallback, l3)); } static void keepAnatomy(Body *b, Skeleton *out, bool cross_legged, float dt) { Vec3 *j = out->joints; static const int UPPER[] = { JOINT_NECK, JOINT_HEAD, JOINT_SHOULDER_L, JOINT_ELBOW_L, JOINT_WRIST_L, JOINT_HAND_L, JOINT_SHOULDER_R, JOINT_ELBOW_R, JOINT_WRIST_R, JOINT_HAND_R }; static const int ARM_L[] = { JOINT_ELBOW_L, JOINT_WRIST_L, JOINT_HAND_L }, ARM_R[] = { JOINT_ELBOW_R, JOINT_WRIST_R, JOINT_HAND_R }; static const int LEG_L[] = { JOINT_KNEE_L, JOINT_ANKLE_L, JOINT_TOE_L }, LEG_R[] = { JOINT_KNEE_R, JOINT_ANKLE_R, JOINT_TOE_R }; Vec3 up = out->head_up, side = vec3_cross(out->chest_forward, up); if (vec3_length(side) < 1e-3f) side = vec3(1.0f, 0.0f, 0.0f); side = vec3_normalize(side); if (side.x < 0.0f) side = vec3_scale(side, -1.0f); /* the trunk: spine and neck keep their length, shoulders and hips their width */ atLeast(j, JOINT_PELVIS, JOINT_CHEST, SPINE * 0.93f, up, UPPER, 10); atLeast(j, JOINT_CHEST, JOINT_NECK, NECK * 0.92f, up, UPPER + 1, 1); j[JOINT_HEAD] = vec3_add(j[JOINT_NECK], vec3_add(vec3_scale(out->head_up, HEAD_UP), vec3_scale(out->head_forward, 0.015f))); atLeast(j, JOINT_CHEST, JOINT_SHOULDER_L, SHOULDER_X * 0.92f, vec3_scale(side, -1.0f), ARM_L, 3); atLeast(j, JOINT_CHEST, JOINT_SHOULDER_R, SHOULDER_X * 0.92f, side, ARM_R, 3); atLeast(j, JOINT_PELVIS, JOINT_HIP_L, HIP_X * 0.95f, vec3_scale(side, -1.0f), LEG_L, 3); atLeast(j, JOINT_PELVIS, JOINT_HIP_R, HIP_X * 0.95f, side, LEG_R, 3); /* limbs */ Vec3 back = vec3_scale(out->chest_forward, -1.0f); /* (lying down or getting up the legs fold their own way: knees just keep turning smoothly) */ bool knee_rule = !cross_legged && b->pose != POSE_LYING_DOWN && b->pose != POSE_GETTING_UP && b->pose != POSE_DOWN; reLimb(j, JOINT_HIP_L, THIGH, SHIN, footSpan(), out->chest_forward, knee_rule, &b->limb_pole[0], &b->limb_pole_ok[0], dt); reLimb(j, JOINT_HIP_R, THIGH, SHIN, footSpan(), out->chest_forward, knee_rule, &b->limb_pole[1], &b->limb_pole_ok[1], dt); reLimb(j, JOINT_SHOULDER_L, UPPER_ARM, FOREARM, HAND, vec3_add(back, vec3_scale(side, -0.4f)), false, &b->limb_pole[2], &b->limb_pole_ok[2], dt); reLimb(j, JOINT_SHOULDER_R, UPPER_ARM, FOREARM, HAND, vec3_add(back, vec3_scale(side, 0.4f)), false, &b->limb_pole[3], &b->limb_pole_ok[3], dt); } /* How deep a leg (hip, knee, ankle, world) goes into what lies about, a limb's thickness round it (0: clear). */ static float legDepth(const Body *b, Vec3 hip, Vec3 knee, Vec3 ankle, float s) { const float r = LEG_RADIUS * s; float most = 0.0f; for (int k = 1; k <= 6; k++) { Vec3 p = k <= 3 ? vec3_lerp(hip, knee, (float)k / 3.0f) : vec3_lerp(knee, ankle, (float)(k - 3) / 4.0f); const float from = p.y - r - 0.05f; /* (whatever reaches above the limb's underside here) */ float q = b->clutter(b->clutter_ctx, p.x, p.z, r, from); if (q > 0.0f) most = fmaxf(most, from + q + r - p.y); } return most; } /* Legs never go through what lies about: a knee that would go through swings out round the leg instead (the hip turns out, as stepping over a fence), as little as will do, and smoothly; if that won't do, the foot comes up off the ground. What still can't be kept clear is a scrape. */ static void clearLegs(Body *b, Vec3 position, Vec3 facing, float s, float dt, Skeleton *out) { b->scrape = 0.0f; bool standing = b->pose == POSE_UP && b->sit < 0.05f && (!b->on_ladder || b->pulling_up) && b->clutter; for (int side = 0; side < 2; side++) { if (!standing) { b->leg_raise[side] = 0.0f; b->knee_off[side] = vec3_zero(); continue; } const int hip_j = side == 0 ? JOINT_HIP_L : JOINT_HIP_R; Vec3 hip = Figure_toWorld(position, facing, out->joints[hip_j]); Vec3 knee = Figure_toWorld(position, facing, out->joints[hip_j + 1]); Vec3 ankle = Figure_toWorld(position, facing, out->joints[hip_j + 2]); Vec3 axis = vec3_sub(ankle, hip); float len = vec3_length(axis); if (len < 1e-3f) continue; axis = vec3_scale(axis, 1.0f / len); /* the knee's place round the leg: along it, and out from it (bend); swung by an angle towards outwards (away from the other leg) */ Vec3 rel = vec3_sub(knee, hip); Vec3 along = vec3_scale(axis, vec3_dot(rel, axis)), bend = vec3_sub(rel, along); float reach = vec3_length(bend); Vec3 outward = vec3_scale(vec3_normalize(vec3_cross(facing, vec3(0.0f, 1.0f, 0.0f))), side == 0 ? 1.0f : -1.0f); Vec3 side_dir = vec3_sub(outward, vec3_scale(axis, vec3_dot(outward, axis))); bool can_swing = reach >= 1e-3f && vec3_length(side_dir) >= 0.2f; /* (a straight leg has no knee to swing) */ Vec3 bend_dir = can_swing ? vec3_scale(bend, 1.0f / reach) : vec3_zero(); if (can_swing) side_dir = vec3_normalize(vec3_sub(side_dir, vec3_scale(bend_dir, vec3_dot(side_dir, bend_dir)))); const float d0 = legDepth(b, hip, knee, ankle, s); float want = 0.0f, best = d0; /* out first; in a little only if out won't do (clear as it is: nothing to do) */ for (int k = 1; can_swing && k <= KNEE_OUT_STEPS + KNEE_IN_STEPS && best > 0.0f; k++) { float a = k <= KNEE_OUT_STEPS ? KNEE_OUT_MOST * (float)k / (float)KNEE_OUT_STEPS : -KNEE_IN_MOST * (float)(k - KNEE_OUT_STEPS) / (float)KNEE_IN_STEPS; Vec3 kn = vec3_add(hip, vec3_add(along, vec3_scale(vec3_add(vec3_scale(bend_dir, cosf(a)), vec3_scale(side_dir, sinf(a))), reach))); float d = legDepth(b, hip, kn, ankle, s); if (d < best - 0.005f) { best = d; want = a; } } /* The knee glides there (out quickly, back slowly), never jumps: the shift from where the pose put it moves at a limited speed, and the knee stays where the bones allow (round the leg) */ Vec3 goal = can_swing ? vec3_add(hip, vec3_add(along, vec3_scale(vec3_add(vec3_scale(bend_dir, cosf(want)), vec3_scale(side_dir, sinf(want))), reach))) : knee; Vec3 goal_off = Figure_toLocal(vec3_zero(), facing, vec3_sub(goal, knee)); Vec3 *off = &b->knee_off[side]; Vec3 change = vec3_sub(goal_off, *off); float most = (vec3_length(goal_off) > vec3_length(*off) ? KNEE_OFF_SPEED : KNEE_OFF_SPEED * 0.35f) * s * dt; if (vec3_length(change) > most) change = vec3_scale(vec3_normalize(change), most); *off = vec3_add(*off, change); Vec3 kn = knee; bool swung = vec3_length(*off) > 1e-3f; if (swung) { Vec3 moved = vec3_sub(vec3_add(knee, Figure_toWorld(vec3_zero(), facing, *off)), hip); Vec3 round = vec3_sub(moved, vec3_scale(axis, vec3_dot(moved, axis))); float rl = vec3_length(round); kn = rl > 1e-4f ? vec3_add(hip, vec3_add(along, vec3_scale(round, reach / rl))) : knee; out->joints[hip_j + 1] = Figure_toLocal(position, facing, kn); } float left = swung ? legDepth(b, hip, kn, ankle, s) : d0; /* Still into it (a foot behind something tall, the hips over it): the foot comes up off the ground, knee bent (onto the edge, climbing) */ float lift_want = 0.0f; if (left > LEG_LET_OFF * s) { const float most_up = hip.y - LEG_RAISE_BELOW_HIP * s - ankle.y; /* (the foot stays well below the hip) */ for (int k = 1; k <= 9 && 0.1f * (float)k * s <= most_up; k++) { float up = 0.1f * (float)k * s; Vec3 a2 = vec3_add(ankle, vec3(0.0f, up, 0.0f)), k2, e2; twoBone(hip, a2, THIGH * s, SHIN * s, vec3_sub(kn, hip), &k2, &e2, false); float d = legDepth(b, hip, k2, e2, s); const float sole = e2.y - ANKLE_Y * s; float q = b->clutter(b->clutter_ctx, e2.x, e2.z, 0.06f * s, sole - 0.05f); /* (and the sole over it) */ if (q > 0.0f) d = fmaxf(d, sole - 0.05f + q - sole); if (d < left) { left = d; lift_want = up; } if (d <= LEG_LET_OFF * s) break; } } float *raised = &b->leg_raise[side]; float rate = (lift_want > *raised ? LEG_RAISE_SPEED : LEG_RAISE_SPEED * 0.5f) * s * dt; *raised += clampf(lift_want - *raised, -rate, rate); *raised = fminf(*raised, fmaxf(hip.y - LEG_RAISE_BELOW_HIP * s - ankle.y, 0.0f)); if (*raised > 1e-3f) { Vec3 a2 = vec3_add(ankle, vec3(0.0f, *raised, 0.0f)), k2, e2; twoBone(hip, a2, THIGH * s, SHIN * s, vec3_sub(kn, hip), &k2, &e2, false); Vec3 toe = Figure_toWorld(position, facing, out->joints[hip_j + 3]); out->joints[hip_j + 1] = Figure_toLocal(position, facing, k2); out->joints[hip_j + 2] = Figure_toLocal(position, facing, e2); out->joints[hip_j + 3] = Figure_toLocal(position, facing, vec3_add(toe, vec3_sub(e2, ankle))); left = legDepth(b, hip, k2, e2, s); } b->scrape = fmaxf(b->scrape, left); } } /* How fast each part of the body can move, relative to the body (m/s, scaled): hands and feet fastest, the trunk slowest. */ static float topSpeedOf(int j) { switch (j) { case JOINT_HAND_L: case JOINT_HAND_R: case JOINT_WRIST_L: case JOINT_WRIST_R: return JOINT_TOP_HAND; case JOINT_ANKLE_L: case JOINT_ANKLE_R: case JOINT_TOE_L: case JOINT_TOE_R: return JOINT_TOP_FOOT; case JOINT_KNEE_L: case JOINT_KNEE_R: return JOINT_TOP_MIDDLE; case JOINT_ELBOW_L: case JOINT_ELBOW_R: return JOINT_TOP_ELBOW; default: return JOINT_TOP_TRUNK; } } /* Nothing on the body moves faster than it can: whatever the pose asks for (a sudden change of plan, a glitch), each joint gets there at most this fast; then the limbs are set right again so the bones keep their length. */ static void limitSpeeds(Body *b, float s, float dt, Skeleton *out) { Vec3 *j = out->joints; if (!b->joints_was_ok || dt <= 0.0f) { for (int i = 0; i < JOINT_COUNT; i++) b->joints_was[i] = j[i]; b->joints_was_ok = true; return; } bool limited = false; for (int i = 0; i < JOINT_COUNT; i++) { Vec3 d = vec3_sub(j[i], b->joints_was[i]); float len = vec3_length(d), most = topSpeedOf(i) * s * fminf(dt, 0.1f); if (len > most) { j[i] = vec3_add(b->joints_was[i], vec3_scale(d, most / len)); limited = true; } } if (limited) { /* bones keep their length: legs and arms from the hip / shoulder to the ankle / wrist again */ static const int ROOT[4] = { JOINT_HIP_L, JOINT_HIP_R, JOINT_SHOULDER_L, JOINT_SHOULDER_R }; for (int k = 0; k < 4; k++) { int r = ROOT[k]; bool leg = k < 2; float l1 = (leg ? THIGH : UPPER_ARM) * s, l2 = (leg ? SHIN : FOREARM) * s, l3 = (leg ? footSpan() : HAND) * s; /* (folded right up, the end almost at the root: which way the knee points is anybody's guess, fitting again would flip it round) */ Vec3 tail = vec3_sub(j[r + 3], j[r + 2]); Vec3 end = refitLimb(j, r, l1, l2, vec3_sub(j[r + 1], j[r])); float tl = vec3_length(tail); if (tl > 1e-4f) j[r + 3] = vec3_add(end, vec3_scale(tail, l3 / tl)); } } for (int i = 0; i < JOINT_COUNT; i++) b->joints_was[i] = j[i]; } /* * Falling, lying, getting up: no part of the body goes into what it came down * on. A joint (or the middle of a bone) under the top of something is lifted * onto it; then arms and legs are fitted again so the bones keep their length. * (High things: the landing was chosen to stay clear of them.) */ #define CONTACT_RISE 5.0f /* m/s a joint comes up onto what it lies on, or down again (quick, but no jump) */ static void keepBodyOut(Body *b, Vec3 position, Vec3 facing, float s, float dt, bool last, Skeleton *out) { /* (sitting: only the legs, they rest on what lies in front instead of going into it) */ const bool legs_only = b->pose == POSE_UP; if ((legs_only && b->sit < 0.05f) || !b->ground) { for (int i = 0; i < JOINT_COUNT; i++) b->lifted[i] = 0.0f; return; } static const int BONES[][2] = { { JOINT_PELVIS, JOINT_CHEST }, { JOINT_CHEST, JOINT_NECK }, { JOINT_NECK, JOINT_HEAD }, { JOINT_PELVIS, JOINT_HIP_L }, { JOINT_HIP_L, JOINT_KNEE_L }, { JOINT_KNEE_L, JOINT_ANKLE_L }, { JOINT_ANKLE_L, JOINT_TOE_L }, { JOINT_PELVIS, JOINT_HIP_R }, { JOINT_HIP_R, JOINT_KNEE_R }, { JOINT_KNEE_R, JOINT_ANKLE_R }, { JOINT_ANKLE_R, JOINT_TOE_R }, { JOINT_CHEST, JOINT_SHOULDER_L }, { JOINT_SHOULDER_L, JOINT_ELBOW_L }, { JOINT_ELBOW_L, JOINT_WRIST_L }, { JOINT_WRIST_L, JOINT_HAND_L }, { JOINT_CHEST, JOINT_SHOULDER_R }, { JOINT_SHOULDER_R, JOINT_ELBOW_R }, { JOINT_ELBOW_R, JOINT_WRIST_R }, { JOINT_WRIST_R, JOINT_HAND_R }, }; Vec3 *j = out->joints; float lift[JOINT_COUNT] = { 0.0f }; bool any = false; const float hips_y = Figure_toWorld(position, facing, j[JOINT_PELVIS]).y; /* (sitting: a table top up there goes over the legs) */ for (unsigned n = 0; n < sizeof BONES / sizeof BONES[0]; n++) { int a = BONES[n][0], c = BONES[n][1]; bool trunk = a <= JOINT_HEAD && c <= JOINT_HEAD; if (legs_only && (a < JOINT_HIP_L || c < JOINT_KNEE_L || c >= JOINT_SHOULDER_L)) continue; /* (from the knee down) */ float r = (trunk ? (c == JOINT_HEAD ? 0.09f : 0.11f) : 0.05f) * s; for (int k = 0; k <= 3; k++) { float t = (float)k / 3.0f; Vec3 w = Figure_toWorld(position, facing, vec3_lerp(j[a], j[c], t)); float from = w.y + 0.55f * s; /* (what a body could come to lie on: about knee high) */ float top = b->ground(b->clutter_ctx, w.x, w.z, from); if (top >= from - 0.01f || top + r <= w.y) continue; /* (clear, or inside something tall: left alone) */ if (legs_only && top > hips_y - 0.05f * s) continue; /* (under a table: the legs go under it, not up on it) */ float need = top + r - w.y; /* a joint is lifted by what's under it, a bone's middle lifts both ends */ if (t < 0.99f) lift[a] = fmaxf(lift[a], need * (1.0f - t) + (k ? need * t : 0.0f)); if (t > 0.01f) lift[c] = fmaxf(lift[c], need * t + (k < 3 ? need * (1.0f - t) : 0.0f)); any = true; } } if (!any) { bool was = false; for (int i = 0; i < JOINT_COUNT; i++) was = was || b->lifted[i] > 0.0f; if (!was) return; /* (and still easing down from a lift otherwise) */ } /* An arm or leg comes up as a whole (the knee or elbow follows from the ends), and takes the hips or the chest along; the trunk stays in one piece */ for (int r = JOINT_HIP_L; r < JOINT_COUNT; r += 4) { float limb = fmaxf(fmaxf(lift[r], lift[r + 1]), fmaxf(lift[r + 2], lift[r + 3])); for (int i = r; i < r + 4; i++) lift[i] = limb; if (legs_only) { lift[r] = 0.0f; continue; } /* (the hips stay on the seat; the leg bends) */ int base = r < JOINT_SHOULDER_L ? JOINT_PELVIS : JOINT_CHEST; lift[base] = fmaxf(lift[base], 0.8f * limb); } float trunk = fmaxf(lift[JOINT_PELVIS], lift[JOINT_CHEST]); lift[JOINT_PELVIS] = fmaxf(lift[JOINT_PELVIS], 0.6f * trunk); lift[JOINT_CHEST] = fmaxf(lift[JOINT_CHEST], 0.6f * trunk); lift[JOINT_NECK] = fmaxf(lift[JOINT_NECK], 0.8f * lift[JOINT_CHEST]); lift[JOINT_HEAD] = fmaxf(lift[JOINT_HEAD], lift[JOINT_NECK]); for (int r = JOINT_HIP_L; r < JOINT_COUNT; r += 4) { /* (hips and shoulders go with the trunk) */ float base = lift[r < JOINT_SHOULDER_L ? JOINT_PELVIS : JOINT_CHEST]; if (base > lift[r]) for (int i = r; i < r + 4; i++) lift[i] = fmaxf(lift[i], base * (i == r ? 1.0f : 0.5f)); } for (int r = JOINT_HIP_L; r < JOINT_COUNT; r += 4) { /* a folded limb can't be fitted again: it comes up as one */ float l = r < JOINT_SHOULDER_L ? THIGH + SHIN : UPPER_ARM + FOREARM; if (vec3_length(vec3_sub(j[r + 2], j[r])) >= FOLDED * l * s || legs_only) continue; float most = fmaxf(fmaxf(lift[r], lift[r + 1]), fmaxf(lift[r + 2], lift[r + 3])); for (int i = r; i < r + 4; i++) lift[i] = most; } float amount = legs_only ? smoothstepf(0.05f, 0.4f, b->sit) : 1.0f; /* (getting up, the legs take over again gently) */ for (int i = 0; i < JOINT_COUNT; i++) { float step = CONTACT_RISE * s * fmaxf(dt, 0.0f); /* (from last frame's, up or down) */ float l = clampf(lift[i] * amount, b->lifted[i] - step, b->lifted[i] + step); j[i].y += l; if (last) b->lifted[i] = l; } /* bones keep their length: legs and arms from the hip / shoulder again */ static const int ROOT[4] = { JOINT_HIP_L, JOINT_HIP_R, JOINT_SHOULDER_L, JOINT_SHOULDER_R }; for (int k = 0; k < 4; k++) { int r = ROOT[k]; bool leg = k < 2; float l1 = (leg ? THIGH : UPPER_ARM) * s, l2 = (leg ? SHIN : FOREARM) * s, l3 = (leg ? footSpan() : HAND) * s; Vec3 tail = vec3_sub(j[r + 3], j[r + 2]); Vec3 end = refitLimb(j, r, l1, l2, vec3_sub(j[r + 1], j[r])); /* (folded up, cross-legged: as it is) */ float tl = vec3_length(tail); if (tl > 1e-4f) j[r + 3] = vec3_add(end, vec3_scale(tail, l3 / tl)); } } /* * Standing: arms keep out of solid things (a wall you stand against, a table, * the box you climb): an arm that would be in one goes back and out to the * side, as far as it must, and is fitted again (bones keep their length). * Hands busy holding on to something are left where they hold. */ #define ARMS_OUT_SPEED 2.0f /* m/s an arm moves out of (or back after) something it would be in */ static void keepArmsOut(Body *b, Vec3 position, Vec3 facing, float s, float dt, Skeleton *out) { if (b->pose != POSE_UP || !b->push_out) { for (int k = 0; k < 2; k++) { b->arm_out[k] = vec3_zero(); b->elbow_up[k] = b->arm_hold[k] = 0.0f; } return; } Vec3 *j = out->joints; const Vec3 right = vec3_normalize(vec3_cross(facing, vec3(0.0f, 1.0f, 0.0f))); const float step = ARMS_OUT_SPEED * s * fmaxf(dt, 0.0f); for (int side = 0; side < 2; side++) { const int sh = side == 0 ? JOINT_SHOULDER_L : JOINT_SHOULDER_R; const float sign = side == 0 ? -1.0f : 1.0f; Vec3 dir = vec3_normalize(vec3_add(vec3_scale(facing, -0.75f), vec3_scale(right, sign * 0.66f))); /* (holding on where it is - not on its way there or back, nor in something: kept out like any arm) */ const bool holding = b->hand_weight[side] > 0.95f && b->push_out(b->clutter_ctx, Figure_toWorld(position, facing, j[sh + 3]), dir, 0.0f) <= 0.05f * s; /* (on a surface is fine) */ float most = 0.0f; for (int k = 1; k <= 6; k++) { /* elbow, along the forearm, wrist, hand */ if (holding && k >= 5) continue; /* (the hand holds on where it is) */ Vec3 local = k <= 1 ? j[sh + 1] : k <= 4 ? vec3_lerp(j[sh + 1], j[sh + 2], (float)(k - 1) / 3.0f) : j[sh + k - 3]; Vec3 w = Figure_toWorld(position, facing, local); most = fmaxf(most, b->push_out(b->clutter_ctx, w, dir, 0.04f * s)); } /* holding: the elbow swings back, up and out (the forearm over the edge); else the arm moves out */ float up_goal = holding && most > 0.0f ? 1.0f : 0.0f; b->arm_hold[side] += clampf((holding ? 1.0f : 0.0f) - b->arm_hold[side], -6.0f * dt, 6.0f * dt); b->elbow_up[side] += clampf(up_goal - b->elbow_up[side], -4.0f * dt, 4.0f * dt); Vec3 goal = vec3_zero(); if (!holding && most > 0.0f) { most = fminf(most, 0.5f * s); goal = vec3_sub(Figure_toLocal(position, facing, vec3_add(position, vec3_scale(dir, most))), Figure_toLocal(position, facing, position)); } Vec3 d = vec3_sub(goal, b->arm_out[side]); /* (eases there and back: no jumps) */ float dl = vec3_length(d); b->arm_out[side] = dl > step ? vec3_add(b->arm_out[side], vec3_scale(d, step / dl)) : goal; if (vec3_length(b->arm_out[side]) < 1e-4f && b->elbow_up[side] < 1e-3f) continue; /* (taking hold: the hand goes to where it holds, the elbow eases back in - both eased, no jump) */ Vec3 hand_out = vec3_scale(b->arm_out[side], 1.0f - b->arm_hold[side]); j[sh + 1] = vec3_add(j[sh + 1], b->arm_out[side]); j[sh + 2] = vec3_add(j[sh + 2], hand_out); j[sh + 3] = vec3_add(j[sh + 3], hand_out); float l1 = UPPER_ARM * s, l2 = FOREARM * s; Vec3 tail = vec3_sub(j[sh + 3], j[sh + 2]); Vec3 pole = vec3_lerp(vec3_normalize(vec3_sub(j[sh + 1], j[sh])), vec3_normalize(vec3(sign * 0.45f, 0.6f, -0.65f)), b->elbow_up[side]); /* elbow up and back: its own way, folded or not - blended in from the usual fit (no flip half way) */ const float own = smoothstepf(0.3f, 0.7f, b->elbow_up[side]); Vec3 own_mid = j[sh + 1], own_end = j[sh + 2], end = j[sh + 2]; if (own > 0.0f) twoBone(j[sh], j[sh + 2], l1, l2, pole, &own_mid, &own_end, true); if (own < 1.0f) end = refitLimb(j, sh, l1, l2, pole); if (own > 0.0f) { Vec3 mid = vec3_lerp(j[sh + 1], own_mid, own), e = vec3_lerp(end, own_end, own); Vec3 u = vec3_sub(mid, j[sh]); if (vec3_length(u) > 1e-4f) mid = vec3_add(j[sh], vec3_scale(u, l1 / vec3_length(u))); /* (bones keep their length) */ Vec3 v = vec3_sub(e, mid); if (vec3_length(v) > 1e-4f) e = vec3_add(mid, vec3_scale(v, l2 / vec3_length(v))); j[sh + 1] = mid; j[sh + 2] = end = e; } float tl = vec3_length(tail); if (tl > 1e-4f) j[sh + 3] = vec3_add(end, vec3_scale(tail, HAND * s / tl)); } } /* ---------- A shoulder roll (unscaled figure space) ---------- */ /* Curled up like a ball round (0, ROLL_BALL, 0): knees to the chest, chin down, the arms guiding. */ #define ROLL_BALL 0.42f #define ROLL_GROUND 0.06f /* the lowest joint this high (a limb's thickness): rolling on the ground */ static const Vec3 TUCK[JOINT_COUNT] = { [JOINT_PELVIS] = { 0.0f, 0.40f, -0.12f }, [JOINT_CHEST] = { 0.0f, 0.60f, 0.10f }, [JOINT_NECK] = { 0.0f, 0.60f, 0.26f }, [JOINT_HEAD] = { 0.0f, 0.50f, 0.36f }, [JOINT_HIP_L] = { -0.10f, 0.40f, -0.10f }, [JOINT_KNEE_L] = { -0.13f, 0.74f, 0.14f }, [JOINT_ANKLE_L] = { -0.12f, 0.40f, -0.20f }, [JOINT_TOE_L] = { -0.12f, 0.32f, -0.10f }, [JOINT_HIP_R] = { 0.10f, 0.40f, -0.10f }, [JOINT_KNEE_R] = { 0.13f, 0.74f, 0.14f }, [JOINT_ANKLE_R] = { 0.12f, 0.40f, -0.20f }, [JOINT_TOE_R] = { 0.12f, 0.32f, -0.10f }, [JOINT_SHOULDER_L] = { -0.18f, 0.60f, 0.12f }, [JOINT_ELBOW_L] = { -0.24f, 0.44f, 0.26f }, [JOINT_WRIST_L] = { -0.19f, 0.36f, 0.44f }, [JOINT_HAND_L] = { -0.16f, 0.30f, 0.52f }, [JOINT_SHOULDER_R] = { 0.18f, 0.60f, 0.12f }, [JOINT_ELBOW_R] = { 0.24f, 0.44f, 0.26f }, [JOINT_WRIST_R] = { 0.19f, 0.36f, 0.44f }, [JOINT_HAND_R] = { 0.16f, 0.30f, 0.52f }, }; static Vec3 yawPoint(Vec3 p, float a) { return vec3(p.x * cosf(a) + p.z * sinf(a), p.y, -p.x * sinf(a) + p.z * cosf(a)); } /* The ball turned over forward by angle (rad), along dir (figure space), the leading shoulder (side) first. */ static void tuckAt(Skeleton *sk, float angle, Vec3 dir, float side) { const float heading = atan2f(dir.x, dir.z), twist = -side * 0.5f, lean = side * 0.3f; for (int i = 0; i < JOINT_COUNT; i++) { Vec3 p = yawPoint(TUCK[i], twist); /* (one shoulder ahead) */ Vec3 q = vec3(p.x, p.y - ROLL_BALL, p.z); q = vec3(q.x * cosf(lean) - q.y * sinf(lean), q.x * sinf(lean) + q.y * cosf(lean), q.z); /* (over onto that side) */ q = vec3(q.x, q.y * cosf(angle) - q.z * sinf(angle), q.y * sinf(angle) + q.z * cosf(angle)); /* forward over */ sk->joints[i] = yawPoint(vec3(q.x, q.y + ROLL_BALL, q.z), heading); } float low = 1e9f; /* (the ball on the ground, not over it) */ for (int i = 0; i < JOINT_COUNT; i++) low = fminf(low, sk->joints[i].y); for (int i = 0; i < JOINT_COUNT; i++) sk->joints[i].y += ROLL_GROUND - low; Vec3 c = sk->joints[JOINT_CHEST], pe = sk->joints[JOINT_PELVIS]; sk->chest_forward = vec3_normalize(yawPoint(vec3(0.0f, -cosf(angle + 0.3f), cosf(angle) * 0.2f + 0.8f), heading)); sk->waist_bend = vec3_scale(vec3_sub(c, pe), 0.0f); } /* Down into the ball, over the shoulder, and up out of it (blended with the standing pose at both ends). */ static void solveShoulderRoll(const Body *b, Vec3 facing, Skeleton *out) { float t = b->shoulder_roll; Vec3 dir = Figure_toLocal(vec3_zero(), facing, b->roll_dir); dir.y = 0.0f; dir = vec3_length(dir) > 1e-3f ? vec3_normalize(dir) : vec3(0.0f, 0.0f, 1.0f); Skeleton tuck = *out; float over = 2.0f * PI_F * smoothstepf(0.2f, 0.8f, t); tuckAt(&tuck, over, dir, b->roll_shoulder); float in = smoothstepf(0.0f, 0.22f, t) * (1.0f - smoothstepf(0.78f, 1.0f, t)); blendSkeleton(out, &tuck, in); } void Body_solve(Body *b, Vec3 position, Vec3 facing, float height, float dt, Skeleton *out) { const float s = height / REF_HEIGHT; solveStanding(b, position, facing, s, dt, out); if (b->pose != POSE_UP) { Vec3 dl = flat(Figure_toLocal(vec3_zero(), facing, b->fall_dir)); /* (no way to fall known - down before ever facing anywhere: back, as when lying down) */ Vec3 d = vec3_length(dl) > 1e-3f && isfinite(dl.x) && isfinite(dl.z) ? vec3_normalize(dl) : vec3(0.0f, 0.0f, -1.0f); Skeleton lying = *out, kneeling = *out; solveLying(b, d, s, &lying); solveKneeling(&kneeling); float t; switch (b->pose) { case POSE_FALLING: { t = clampf(b->pose_time / fmaxf(b->fall_time, 0.1f), 0.0f, 1.0f); float topple = t; if (b->from_knees) *out = kneeling; /* gave way while getting up */ else if (b->fall_style == FALL_CRUMPLE) { /* The knees go first, then the rest */ blendSkeleton(out, &kneeling, 0.8f * smoothstepf(0.0f, 0.5f, t)); topple = smoothstepf(0.25f, 1.0f, t); } else if (b->fall_style == FALL_DIVE) { /* Down quick: knees bend, then forward onto the hands and flat */ blendSkeleton(out, &kneeling, 0.6f * smoothstepf(0.0f, 0.3f, t)); topple = smoothstepf(0.1f, 0.9f, t); } turnAbout(out, b->fall_twist * smoothstepf(0.0f, 0.8f, t)); fallOver(out, d, topple); blendSkeleton(out, &lying, smoothstepf(0.55f, 1.0f, t)); break; } case POSE_LYING_DOWN: t = clampf(b->pose_time / LIE_DOWN_TIME, 0.0f, 1.0f); if (t < 0.5f) blendSkeleton(out, &kneeling, smoothstepf(0.0f, 0.5f, t)); else { *out = kneeling; blendSkeleton(out, &lying, smoothstepf(0.5f, 1.0f, t)); } break; case POSE_DOWN: *out = lying; break; case POSE_GETTING_UP: { t = clampf(b->pose_time / GET_UP_TIME, 0.0f, 1.0f); Skeleton standing = *out; if (t < 0.5f) { *out = lying; blendSkeleton(out, &kneeling, smoothstepf(0.0f, 0.5f, t)); } else { *out = kneeling; blendSkeleton(out, &standing, smoothstepf(0.5f, 1.0f, t)); } break; } case POSE_UP: break; } } if (b->pose == POSE_UP && b->sit > 0.01f) { Skeleton sitting = *out; solveSitting(&sitting, b->sit_style, b->seat_height / s, sinf(b->clock * 1.6f)); for (int side = 0; side < 2; side++) { /* busy hands still do their thing sitting down (smoking too) */ float w = clampf(b->hand_weight[side], 0.0f, 1.0f); int sh = side == 0 ? JOINT_SHOULDER_L : JOINT_SHOULDER_R; Vec3 smoke_palm, smoke_pole; float smoke = smokeArm(b, sitting.joints, &sitting, side, &smoke_palm, &smoke_pole) * (1.0f - w); if (smoke > 0.001f) { Vec3 at = vec3_lerp(sitting.joints[sh + 3], smoke_palm, clampf(smoke, 0.0f, 1.0f)); reachArm(sitting.joints, sh, at, smoke_pole); } if (w < 0.001f || !b->hand_ready[side]) continue; Vec3 palm = vec3_lerp(sitting.joints[sh + 3], b->hand_local[side], w); Vec3 rel = vec3_sub(palm, sitting.joints[sh]); if (vec3_length(rel) < 0.2f) palm = vec3_add(sitting.joints[sh], vec3_scale(vec3_normalize(vec3_add(rel, vec3(0.0f, -0.05f, 0.1f))), 0.2f)); reachArm(sitting.joints, sh, palm, reachPole(sitting.joints[sh], palm, side == 0 ? -1.0f : 1.0f)); } blendSkeleton(out, &sitting, smoothstepf(0.0f, 1.0f, b->sit)); } keepAnatomy(b, out, b->pose == POSE_UP && b->sit > 0.05f && b->sit_style == SIT_GROUND, dt); const bool rolling = b->shoulder_roll > 0.0f && b->pose == POSE_UP; if (rolling) solveShoulderRoll(b, facing, out); out->face = faceFor(b); StreamArc arc = {0}; solveStream(b, facing, out, &arc); for (int i = 0; i < JOINT_COUNT; i++) out->joints[i] = vec3_scale(out->joints[i], s); out->waist_bend = vec3_scale(out->waist_bend, s); for (int k = 0; k < 4; k++) out->stream[k] = vec3_scale(out->stream[k], s); out->uri_a = vec3_scale(out->uri_a, s); out->uri_b = vec3_scale(out->uri_b, s); out->stream_width *= s; landStream(b, position, facing, s, &arc, out); if (!rolling) { /* (a set move: as it is) */ keepBodyOut(b, position, facing, s, dt, false, out); clearLegs(b, position, facing, s, dt, out); limitSpeeds(b, s, dt, out); keepBodyOut(b, position, facing, s, dt, true, out); /* (contact wins over the speed limit) */ keepArmsOut(b, position, facing, s, dt, out); } placeCigarette(b, s, out); b->shown_valid = b->pose == POSE_UP && b->sit < 0.02f && !b->airborne; if (b->shown_valid) { b->ankle_shown[0] = Figure_toWorld(position, facing, out->joints[JOINT_ANKLE_L]); b->ankle_shown[1] = Figure_toWorld(position, facing, out->joints[JOINT_ANKLE_R]); b->toe_shown[0] = Figure_toWorld(position, facing, out->joints[JOINT_TOE_L]); b->toe_shown[1] = Figure_toWorld(position, facing, out->joints[JOINT_TOE_R]); } }