@bitefoundry/retrieviewer 0.0.2 → 0.0.4
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
- package/ASSET-PROVENANCE.md +6 -0
- package/ASSETS-LICENSE.md +2 -1
- package/ATTRIBUTION.md +6 -1
- package/COMFORT-ART.md +65 -0
- package/DESKTOP-PLATFORMS.md +75 -0
- package/INSTALLING.md +8 -5
- package/PLUGIN.md +13 -10
- package/PREREQUISITES.md +95 -0
- package/PUPPY-MEMORY.md +102 -1
- package/README.ko.md +110 -0
- package/README.md +119 -58
- package/RELEASING.md +27 -14
- package/THIRD-PARTY-NOTICES.md +5 -3
- package/braille.mjs +35 -9
- package/examples/live.mjs +2 -2
- package/integrations/desktop/platform.mjs +27 -0
- package/integrations/linux-overlay/README.md +83 -0
- package/integrations/linux-overlay/overlay.py +1094 -0
- package/integrations/macos-overlay/README.md +83 -0
- package/integrations/macos-overlay/overlay.jxa +639 -0
- package/integrations/opencode/runtime.mjs +4 -4
- package/integrations/opencode/tui.tsx +1 -1
- package/integrations/windows-overlay/Overlay.cs +4 -4
- package/integrations/windows-overlay/desktop.mjs +40 -8
- package/integrations/windows-overlay/protocol.mjs +11 -7
- package/integrations/windows-overlay/worker.mjs +13 -7
- package/package.json +41 -14
- package/puppy-comfort-sprites.mjs +25 -0
- package/puppy-companion-comfort-q5.bin.gz +0 -0
- package/puppy-companion-comfort.json +77 -0
- package/puppy-companion-life.mjs +49 -16
- package/puppy-companion-objects.mjs +12 -0
- package/puppy-frame-predictive.mjs +15 -9
- package/puppy-frame-store.mjs +2 -2
- package/puppy-paper-world.mjs +14 -0
- package/puppy-paper.mjs +65 -14
- package/retrieviewer-cli.mjs +10 -3
- package/retrieviewer-plugin.d.mts +1 -1
- package/retrieviewer-plugin.mjs +1 -1
- package/retrieviewer.d.mts +10 -4
- package/retrieviewer.mjs +20 -14
- package/locomotion-core.mjs +0 -21
- package/locomotion-puppy.animal +0 -0
- package/vendor/procedural-animals/package.json +0 -40
- package/vendor/procedural-animals/src/animal.js +0 -146
- package/vendor/procedural-animals/src/bake.js +0 -51
- package/vendor/procedural-animals/src/build.js +0 -81
- package/vendor/procedural-animals/src/core/build/coatKit.js +0 -121
- package/vendor/procedural-animals/src/core/build/featherCards.js +0 -189
- package/vendor/procedural-animals/src/core/build/finEdges.js +0 -85
- package/vendor/procedural-animals/src/core/build/furSlope.js +0 -115
- package/vendor/procedural-animals/src/core/build/mesh.js +0 -67
- package/vendor/procedural-animals/src/core/build/pipeline.js +0 -272
- package/vendor/procedural-animals/src/core/build/seams.js +0 -183
- package/vendor/procedural-animals/src/core/build/warp.js +0 -112
- package/vendor/procedural-animals/src/core/build/weights.js +0 -393
- package/vendor/procedural-animals/src/core/math/vec.js +0 -81
- package/vendor/procedural-animals/src/core/motion/actions.js +0 -800
- package/vendor/procedural-animals/src/core/motion/bird.js +0 -3063
- package/vendor/procedural-animals/src/core/motion/birdActions.js +0 -1059
- package/vendor/procedural-animals/src/core/motion/common.js +0 -158
- package/vendor/procedural-animals/src/core/motion/index.js +0 -22
- package/vendor/procedural-animals/src/core/motion/quadruped.js +0 -2875
- package/vendor/procedural-animals/src/core/motion/snake.js +0 -1594
- package/vendor/procedural-animals/src/core/motion/spider.js +0 -1810
- package/vendor/procedural-animals/src/core/motion/swimmer.js +0 -1047
- package/vendor/procedural-animals/src/core/motion/swimmerActions.js +0 -613
- package/vendor/procedural-animals/src/core/render/animalObject.js +0 -262
- package/vendor/procedural-animals/src/core/render/coatMaterial.js +0 -1301
- package/vendor/procedural-animals/src/core/render/dqs.js +0 -159
- package/vendor/procedural-animals/src/core/render/eyes.js +0 -676
- package/vendor/procedural-animals/src/core/render/membranes.js +0 -77
- package/vendor/procedural-animals/src/core/render/skeleton.js +0 -41
- package/vendor/procedural-animals/src/core/rig/bird.js +0 -327
- package/vendor/procedural-animals/src/core/rig/quadruped.js +0 -76
- package/vendor/procedural-animals/src/core/rig/rig.js +0 -79
- package/vendor/procedural-animals/src/core/rig/snake.js +0 -76
- package/vendor/procedural-animals/src/core/rig/spider.js +0 -110
- package/vendor/procedural-animals/src/core/rig/swimmer.js +0 -181
- package/vendor/procedural-animals/src/core/sdf/eyeSocket.js +0 -53
- package/vendor/procedural-animals/src/core/sdf/mesher.js +0 -238
- package/vendor/procedural-animals/src/core/sdf/sdf.js +0 -263
- package/vendor/procedural-animals/src/index.d.ts +0 -98
- package/vendor/procedural-animals/src/index.js +0 -29
- package/vendor/procedural-animals/src/registry.js +0 -48
- package/vendor/procedural-animals/src/species/bear/behaviour.js +0 -463
- package/vendor/procedural-animals/src/species/bear/claws.js +0 -147
- package/vendor/procedural-animals/src/species/bear/coat.js +0 -726
- package/vendor/procedural-animals/src/species/bear/index.js +0 -139
- package/vendor/procedural-animals/src/species/bear/motion.js +0 -112
- package/vendor/procedural-animals/src/species/bear/regions.js +0 -78
- package/vendor/procedural-animals/src/species/bear/rig.js +0 -112
- package/vendor/procedural-animals/src/species/bear/sculpt.js +0 -272
- package/vendor/procedural-animals/src/species/boar/behaviour.js +0 -335
- package/vendor/procedural-animals/src/species/boar/coat.js +0 -372
- package/vendor/procedural-animals/src/species/boar/index.js +0 -110
- package/vendor/procedural-animals/src/species/boar/motion.js +0 -85
- package/vendor/procedural-animals/src/species/boar/regions.js +0 -88
- package/vendor/procedural-animals/src/species/boar/rig.js +0 -111
- package/vendor/procedural-animals/src/species/boar/sculpt.js +0 -285
- package/vendor/procedural-animals/src/species/cat/behaviour.js +0 -559
- package/vendor/procedural-animals/src/species/cat/coat.js +0 -841
- package/vendor/procedural-animals/src/species/cat/index.js +0 -211
- package/vendor/procedural-animals/src/species/cat/motion.js +0 -101
- package/vendor/procedural-animals/src/species/cat/regions.js +0 -82
- package/vendor/procedural-animals/src/species/cat/rig.js +0 -100
- package/vendor/procedural-animals/src/species/cat/sculpt.js +0 -266
- package/vendor/procedural-animals/src/species/cat/whiskers.js +0 -110
- package/vendor/procedural-animals/src/species/cheetah/coat.js +0 -427
- package/vendor/procedural-animals/src/species/cheetah/index.js +0 -51
- package/vendor/procedural-animals/src/species/cheetah/joints.js +0 -3
- package/vendor/procedural-animals/src/species/cheetah/motion.js +0 -77
- package/vendor/procedural-animals/src/species/cheetah/regions.js +0 -95
- package/vendor/procedural-animals/src/species/cheetah/rig.js +0 -66
- package/vendor/procedural-animals/src/species/cheetah/sculpt.js +0 -158
- package/vendor/procedural-animals/src/species/chicken/coat.js +0 -313
- package/vendor/procedural-animals/src/species/chicken/coverts.js +0 -320
- package/vendor/procedural-animals/src/species/chicken/index.js +0 -110
- package/vendor/procedural-animals/src/species/chicken/motion.js +0 -177
- package/vendor/procedural-animals/src/species/chicken/regions.js +0 -66
- package/vendor/procedural-animals/src/species/chicken/rig.js +0 -144
- package/vendor/procedural-animals/src/species/chicken/sculpt.js +0 -389
- package/vendor/procedural-animals/src/species/chicken/wingLook.js +0 -66
- package/vendor/procedural-animals/src/species/chicken/wingStack.js +0 -124
- package/vendor/procedural-animals/src/species/cow/behaviour.js +0 -521
- package/vendor/procedural-animals/src/species/cow/coat.js +0 -465
- package/vendor/procedural-animals/src/species/cow/index.js +0 -118
- package/vendor/procedural-animals/src/species/cow/motion.js +0 -81
- package/vendor/procedural-animals/src/species/cow/regions.js +0 -98
- package/vendor/procedural-animals/src/species/cow/rig.js +0 -116
- package/vendor/procedural-animals/src/species/cow/sculpt.js +0 -339
- package/vendor/procedural-animals/src/species/crow/coat.js +0 -145
- package/vendor/procedural-animals/src/species/crow/index.js +0 -70
- package/vendor/procedural-animals/src/species/crow/joints.js +0 -3
- package/vendor/procedural-animals/src/species/crow/motion.js +0 -101
- package/vendor/procedural-animals/src/species/crow/regions.js +0 -50
- package/vendor/procedural-animals/src/species/crow/rig.js +0 -83
- package/vendor/procedural-animals/src/species/crow/sculpt.js +0 -118
- package/vendor/procedural-animals/src/species/deer/behaviour.js +0 -513
- package/vendor/procedural-animals/src/species/deer/coat.js +0 -391
- package/vendor/procedural-animals/src/species/deer/index.js +0 -106
- package/vendor/procedural-animals/src/species/deer/motion.js +0 -97
- package/vendor/procedural-animals/src/species/deer/regions.js +0 -60
- package/vendor/procedural-animals/src/species/deer/rig.js +0 -124
- package/vendor/procedural-animals/src/species/deer/sculpt.js +0 -297
- package/vendor/procedural-animals/src/species/dog/behaviour.js +0 -521
- package/vendor/procedural-animals/src/species/dog/coat.js +0 -495
- package/vendor/procedural-animals/src/species/dog/index.js +0 -144
- package/vendor/procedural-animals/src/species/dog/motion.js +0 -118
- package/vendor/procedural-animals/src/species/dog/regions.js +0 -61
- package/vendor/procedural-animals/src/species/dog/rig.js +0 -129
- package/vendor/procedural-animals/src/species/dog/sculpt.js +0 -246
- package/vendor/procedural-animals/src/species/eagle/coat.js +0 -250
- package/vendor/procedural-animals/src/species/eagle/index.js +0 -76
- package/vendor/procedural-animals/src/species/eagle/joints.js +0 -3
- package/vendor/procedural-animals/src/species/eagle/motion.js +0 -137
- package/vendor/procedural-animals/src/species/eagle/regions.js +0 -57
- package/vendor/procedural-animals/src/species/eagle/rig.js +0 -107
- package/vendor/procedural-animals/src/species/eagle/sculpt.js +0 -137
- package/vendor/procedural-animals/src/species/fish/coat.js +0 -271
- package/vendor/procedural-animals/src/species/fish/index.js +0 -88
- package/vendor/procedural-animals/src/species/fish/motion.js +0 -92
- package/vendor/procedural-animals/src/species/fish/regions.js +0 -27
- package/vendor/procedural-animals/src/species/fish/rig.js +0 -143
- package/vendor/procedural-animals/src/species/fish/sculpt.js +0 -214
- package/vendor/procedural-animals/src/species/fish/variants.js +0 -126
- package/vendor/procedural-animals/src/species/fox/behaviour.js +0 -314
- package/vendor/procedural-animals/src/species/fox/coat.js +0 -444
- package/vendor/procedural-animals/src/species/fox/index.js +0 -120
- package/vendor/procedural-animals/src/species/fox/motion.js +0 -81
- package/vendor/procedural-animals/src/species/fox/regions.js +0 -59
- package/vendor/procedural-animals/src/species/fox/rig.js +0 -91
- package/vendor/procedural-animals/src/species/fox/sculpt.js +0 -202
- package/vendor/procedural-animals/src/species/frog/behaviour.js +0 -590
- package/vendor/procedural-animals/src/species/frog/coat.js +0 -305
- package/vendor/procedural-animals/src/species/frog/index.js +0 -97
- package/vendor/procedural-animals/src/species/frog/motion.js +0 -69
- package/vendor/procedural-animals/src/species/frog/regions.js +0 -46
- package/vendor/procedural-animals/src/species/frog/rig.js +0 -156
- package/vendor/procedural-animals/src/species/frog/sculpt.js +0 -240
- package/vendor/procedural-animals/src/species/goat/behaviour.js +0 -509
- package/vendor/procedural-animals/src/species/goat/coat.js +0 -590
- package/vendor/procedural-animals/src/species/goat/index.js +0 -149
- package/vendor/procedural-animals/src/species/goat/motion.js +0 -95
- package/vendor/procedural-animals/src/species/goat/regions.js +0 -98
- package/vendor/procedural-animals/src/species/goat/rig.js +0 -181
- package/vendor/procedural-animals/src/species/goat/sculpt.js +0 -547
- package/vendor/procedural-animals/src/species/horse/coat.js +0 -361
- package/vendor/procedural-animals/src/species/horse/coatSeam.js +0 -122
- package/vendor/procedural-animals/src/species/horse/index.js +0 -80
- package/vendor/procedural-animals/src/species/horse/motion.js +0 -245
- package/vendor/procedural-animals/src/species/horse/regions.js +0 -60
- package/vendor/procedural-animals/src/species/horse/rig.js +0 -117
- package/vendor/procedural-animals/src/species/horse/sculpt.js +0 -316
- package/vendor/procedural-animals/src/species/horse/seamWeights.js +0 -272
- package/vendor/procedural-animals/src/species/lion/behaviour.js +0 -509
- package/vendor/procedural-animals/src/species/lion/coat.js +0 -1016
- package/vendor/procedural-animals/src/species/lion/index.js +0 -111
- package/vendor/procedural-animals/src/species/lion/motion.js +0 -87
- package/vendor/procedural-animals/src/species/lion/regions.js +0 -106
- package/vendor/procedural-animals/src/species/lion/rig.js +0 -110
- package/vendor/procedural-animals/src/species/lion/sculpt.js +0 -384
- package/vendor/procedural-animals/src/species/lion/whiskers.js +0 -132
- package/vendor/procedural-animals/src/species/pig/behaviour.js +0 -575
- package/vendor/procedural-animals/src/species/pig/coat.js +0 -730
- package/vendor/procedural-animals/src/species/pig/ears.js +0 -242
- package/vendor/procedural-animals/src/species/pig/index.js +0 -161
- package/vendor/procedural-animals/src/species/pig/motion.js +0 -105
- package/vendor/procedural-animals/src/species/pig/regions.js +0 -100
- package/vendor/procedural-animals/src/species/pig/rig.js +0 -152
- package/vendor/procedural-animals/src/species/pig/sculpt.js +0 -382
- package/vendor/procedural-animals/src/species/rabbit/behaviour.js +0 -605
- package/vendor/procedural-animals/src/species/rabbit/coat.js +0 -285
- package/vendor/procedural-animals/src/species/rabbit/index.js +0 -85
- package/vendor/procedural-animals/src/species/rabbit/motion.js +0 -85
- package/vendor/procedural-animals/src/species/rabbit/regions.js +0 -48
- package/vendor/procedural-animals/src/species/rabbit/rig.js +0 -99
- package/vendor/procedural-animals/src/species/rabbit/sculpt.js +0 -148
- package/vendor/procedural-animals/src/species/rat/behaviour.js +0 -539
- package/vendor/procedural-animals/src/species/rat/coat.js +0 -341
- package/vendor/procedural-animals/src/species/rat/index.js +0 -111
- package/vendor/procedural-animals/src/species/rat/motion.js +0 -90
- package/vendor/procedural-animals/src/species/rat/regions.js +0 -92
- package/vendor/procedural-animals/src/species/rat/rig.js +0 -112
- package/vendor/procedural-animals/src/species/rat/sculpt.js +0 -163
- package/vendor/procedural-animals/src/species/rat/whiskers.js +0 -119
- package/vendor/procedural-animals/src/species/shark/coat.js +0 -383
- package/vendor/procedural-animals/src/species/shark/index.js +0 -104
- package/vendor/procedural-animals/src/species/shark/motion.js +0 -69
- package/vendor/procedural-animals/src/species/shark/regions.js +0 -34
- package/vendor/procedural-animals/src/species/shark/rig.js +0 -194
- package/vendor/procedural-animals/src/species/shark/sculpt.js +0 -343
- package/vendor/procedural-animals/src/species/shark/variants.js +0 -105
- package/vendor/procedural-animals/src/species/sheep/behaviour.js +0 -522
- package/vendor/procedural-animals/src/species/sheep/coat.js +0 -529
- package/vendor/procedural-animals/src/species/sheep/index.js +0 -152
- package/vendor/procedural-animals/src/species/sheep/motion.js +0 -86
- package/vendor/procedural-animals/src/species/sheep/neck.js +0 -41
- package/vendor/procedural-animals/src/species/sheep/regions.js +0 -62
- package/vendor/procedural-animals/src/species/sheep/rig.js +0 -163
- package/vendor/procedural-animals/src/species/sheep/sculpt.js +0 -542
- package/vendor/procedural-animals/src/species/snake/coat.js +0 -315
- package/vendor/procedural-animals/src/species/snake/index.js +0 -73
- package/vendor/procedural-animals/src/species/snake/motion.js +0 -51
- package/vendor/procedural-animals/src/species/snake/regions.js +0 -51
- package/vendor/procedural-animals/src/species/snake/rig.js +0 -108
- package/vendor/procedural-animals/src/species/snake/sculpt.js +0 -122
- package/vendor/procedural-animals/src/species/spider/coat.js +0 -279
- package/vendor/procedural-animals/src/species/spider/index.js +0 -78
- package/vendor/procedural-animals/src/species/spider/motion.js +0 -72
- package/vendor/procedural-animals/src/species/spider/regions.js +0 -87
- package/vendor/procedural-animals/src/species/spider/rig.js +0 -118
- package/vendor/procedural-animals/src/species/spider/sculpt.js +0 -182
- package/vendor/procedural-animals/src/species/wolf/behaviour.js +0 -160
- package/vendor/procedural-animals/src/species/wolf/coat.js +0 -749
- package/vendor/procedural-animals/src/species/wolf/index.js +0 -107
- package/vendor/procedural-animals/src/species/wolf/motion.js +0 -80
- package/vendor/procedural-animals/src/species/wolf/regions.js +0 -57
- package/vendor/procedural-animals/src/species/wolf/rig.js +0 -87
- package/vendor/procedural-animals/src/species/wolf/sculpt.js +0 -339
- package/vendor/procedural-animals/src/worker.js +0 -29
|
@@ -1,2875 +0,0 @@
|
|
|
1
|
-
// Quadruped motion engine: a data-driven generalisation of the original procedural cheetah.
|
|
2
|
-
//
|
|
3
|
-
// Gait engine (phase-offset table blended by speed), foot planting with terrain-aware swing paths,
|
|
4
|
-
// pantograph hind-leg IK, foreleg IK with a sliding scapula (best-fit protraction), a flexing spine,
|
|
5
|
-
// banking, a one-arc neck with head stabilisation that rises with speed, a spring-chain tail,
|
|
6
|
-
// ears, jaw, breathing and panting. Actions (core/motion/actions.js) blend a posture parameter set
|
|
7
|
-
// over the locomotion every frame.
|
|
8
|
-
//
|
|
9
|
-
// Lessons kept from the cheetah: soft IK reach limits, continuous solves (nothing jumps between
|
|
10
|
-
// frames), body bounce from the gait phase (not from foot events), one-arc neck, spring-chain tail,
|
|
11
|
-
// head stabilisation rising with speed, zero-dt safety.
|
|
12
|
-
//
|
|
13
|
-
// Every length is read from the packed joints (data.joints: final, warped and scaled) or given in
|
|
14
|
-
// species data at the reference size and multiplied by s = params.size. Speeds in tables are at the
|
|
15
|
-
// reference size: the gait lookup uses v / sqrt(s), stride frequencies are divided by sqrt(s)
|
|
16
|
-
// (dynamic similarity, equal Froude number).
|
|
17
|
-
//
|
|
18
|
-
// ------------------------------------------------------------------------------------------------
|
|
19
|
-
// species.motion schema (every field optional unless marked; defaults in DEFAULTS below)
|
|
20
|
-
// ------------------------------------------------------------------------------------------------
|
|
21
|
-
// unit the reference individual's leg length relative to the cheetah's (default: measured from
|
|
22
|
-
// the bind skeleton, see refLegLength): engine constants scale by cfg.k = size x unit
|
|
23
|
-
// maxSpeed m/s top speed (reference size) accel [slow, fast] m/s^2, decel m/s^2
|
|
24
|
-
// turnRate rad/s max yaw rate latAccelMax m/s^2 cornering limit
|
|
25
|
-
// (turns: see step and turnCentre; the body leans into them from a trot on)
|
|
26
|
-
// gears { name: speed } named speeds for UIs (walk, trot, ...)
|
|
27
|
-
// gaits (req.) [{ name, v, f, D, off: [FL, FR, HL, HR], liftF, liftH, bobF, bobH, flex, lat, fold,
|
|
28
|
-
// heel, tail, drop, lead?, width? }] sorted by v. v: speed where the row applies, f: stride Hz,
|
|
29
|
-
// D: duty factor, off: touchdown phase per leg, lift*: swing height (m), bob*: girdle
|
|
30
|
-
// bounce (m), flex: spine flexion amplitude (rad; the table's largest also sets how
|
|
31
|
-
// far the spine may bend sideways in a turn), lat: lateral spine wave (rad),
|
|
32
|
-
// width: stance width factor (single-tracking trot < 1, default 1),
|
|
33
|
-
// fold: carpal fold in swing (rad), heel: heel lift at push-off (deg), tail: tail
|
|
34
|
-
// activity 0..1, drop: body lowering (m), lead: extra landing lead (m).
|
|
35
|
-
// Hind pairs landing together (bound, half-bound, frog hop): give HL and HR the same
|
|
36
|
-
// offset (and FL / FR for a pronk); nothing else is needed.
|
|
37
|
-
// gallopBlend [v0, v1] speeds over which the body style goes from walking to galloping
|
|
38
|
-
// flexBlend [v0, v1] speeds over which the gait's spine flexion fades in ([3.5, 6]; hoppers flex
|
|
39
|
-
// their back in every hop: rabbit [0.3, 1.5])
|
|
40
|
-
// bankScale share of the dynamic lean into a turn (default 1: atan(lateral acceleration / g) x 0.85);
|
|
41
|
-
// a heavy animal that corners hard without leaning like a runner lowers it (bear 0.55)
|
|
42
|
-
// reachDuty 1 (default): a leg's stance may not sweep further than it reaches (the duty factor is
|
|
43
|
-
// shortened per leg where v D / f exceeds the reachable sweep); 0 disables, > 1 allows
|
|
44
|
-
// a longer (dragged) stance
|
|
45
|
-
// feet { front, hind }: { type: 'digitigrade' | 'unguligrade' | 'plantigrade',
|
|
46
|
-
// contact: 0..1 where the ground contact lies between the foot's back joint and the
|
|
47
|
-
// toe tip, curl: swing toe curl / hoof flip (deg), flex: stance carpal flex (deg),
|
|
48
|
-
// couple: share of the toe curl / heel lift the metacarpus follows (paws 0.6, hooves 0.2),
|
|
49
|
-
// swingBack: the swing path bows back by this much (m, reference size) at mid swing,
|
|
50
|
-
// sink: fetlock sink under load (deg, hooves), hoofFlex: extra coffin-joint flexion
|
|
51
|
-
// in swing (deg, hooves with a hoof bone), heelStrike: toe-up angle at touchdown
|
|
52
|
-
// (deg, plantigrade), lead: max landing lead / leg length, liftBehind: early lift-off
|
|
53
|
-
// distance behind the girdle / leg length, reach: IK reach limit / leg length,
|
|
54
|
-
// retract: 0..1 swing retraction (the foot meets the ground at ~zero ground speed:
|
|
55
|
-
// small fast animals whose stance lasts 2-3 frames), plantRest (hind): metatarsus flat
|
|
56
|
-
// on the ground at rest and in postures, on the toes in motion (rabbit, rodents),
|
|
57
|
-
// elbowDown (front): tilts the elbow pole down (paws raised to the face: grooming),
|
|
58
|
-
// toeOut: paw yaw away from the midline (rad, default 0.06; < 0 turns the toes in: bear) }
|
|
59
|
-
// Hooves: a hoof bone per leg (fhoof / hhoof from joint fcoffin / hcoffin to the toe,
|
|
60
|
-
// below fpaw / hpaw = pastern) is picked up automatically: the fetlock sinks about the
|
|
61
|
-
// coffin joint in stance and the hoof flips in swing
|
|
62
|
-
// stance { width: x stance width, gallopWidth: width factor at a gallop, sprawl: 0..1 (limbs
|
|
63
|
-
// splay out, knees / elbows point sideways: frogs, lizards), crouch: 0..1 permanent
|
|
64
|
-
// body lowering, scuff: [v0, v1, amount] paw roll at sprint speeds }
|
|
65
|
-
// body { chest: { y, hh, hw }, pelvis: { y, hh, hw } torso sections at the girdles (bind,
|
|
66
|
-
// ref size: centre height, half height, half width), rump: { z, y, hh, hw } the
|
|
67
|
-
// section over the tail root, back: saddle height above the spine (m) }
|
|
68
|
-
// head { carriage: [[v, raise m, reach m, pitch rad]], stab: [[v, 0..1]], neckBend: deg,
|
|
69
|
-
// neckPivot: 0..1 (share of up/down taken at the withers), yaw, pitchUp, pitchDown:
|
|
70
|
-
// look limits (rad), mouth: joint used as the mouth ('jawTip') }
|
|
71
|
-
// tail { carriage: [[v, [deg per segment base -> tip]]] (resampled to the segment count),
|
|
72
|
-
// stiffness, sway (deg, stride sway), flick (cat tip flicks), wag (dog: 0..1),
|
|
73
|
-
// swish (horse / cow: 0..1), curl (curled over the back: deg at the tip), balance
|
|
74
|
-
// (cheetah turn / acceleration model 0..1), radius [base, tip] (m) }
|
|
75
|
-
// ears { rest: [pitch, roll] offsets (rad), mobility: 0..1, speedFlatten: rad at speed,
|
|
76
|
-
// flattenSpeeds: [v0, v1] where they go back ([4, 14]), twitch: 0..1, prick: swivel
|
|
77
|
-
// toward the look target (rad) }
|
|
78
|
-
// breath { rate Hz, amp, pant: bool, pantRate Hz, pantAmp, heatSpeed: speed that heats }
|
|
79
|
-
// actions per-action tuning, see core/motion/actions.js (attack: { style }, eat: { style } ...)
|
|
80
|
-
// attachments extra / overriding attachment points { name: { bone, from: joint, to?: joint, t?,
|
|
81
|
-
// offset?: [x, y, z] m at the reference size } } (animal.attachments[name])
|
|
82
|
-
// variants { <params.variant>: { <key>: ... } } per-variant overrides, merged one level deep
|
|
83
|
-
// hooks { actions, update(P, dt, layer), pose(engine, dt) }: species behaviour, see the
|
|
84
|
-
// "Species hooks" section of core/motion/actions.js
|
|
85
|
-
// Species bones the engine does not know (a snout, a dewlap, horns on their own bone) follow their
|
|
86
|
-
// parent bone rigidly every frame; hooks.pose may move them.
|
|
87
|
-
// ------------------------------------------------------------------------------------------------
|
|
88
|
-
import * as THREE from 'three';
|
|
89
|
-
import {
|
|
90
|
-
TAU, DEG, clamp, lerp, smooth, wrap01, d01, angDiff, smin, Spring, tv, tvc, tq, tqc, tqa,
|
|
91
|
-
UP, X1, Z1, ONE, writeFrame, prng, tableAt,
|
|
92
|
-
} from './common.js';
|
|
93
|
-
import { ActionLayer, createPostureParams } from './actions.js';
|
|
94
|
-
|
|
95
|
-
const FOOT_DEFAULTS = {
|
|
96
|
-
digitigrade: { contact: 0.42, curl: 60, flex: 14, sink: 0, heelStrike: 0, couple: 0.6 },
|
|
97
|
-
unguligrade: { contact: 0.8, curl: 110, flex: 6, sink: 16, heelStrike: 0, couple: 0.2 },
|
|
98
|
-
plantigrade: { contact: 0.45, curl: 35, flex: 10, sink: 0, heelStrike: 14, couple: 0.6 },
|
|
99
|
-
};
|
|
100
|
-
|
|
101
|
-
const DEFAULTS = {
|
|
102
|
-
maxSpeed: 12, accel: [6, 8], decel: 10, turnRate: 2.4, latAccelMax: 10, moveCrouch: 0.05, carpusLock: 1,
|
|
103
|
-
// stride styling: bounce gain (spring-mass), stride roll, stride-to-stride variation and the braking /
|
|
104
|
-
// push-off posture (0..1)
|
|
105
|
-
stride: { bob: 0.5, roll: 1, vary: 1, brake: 1, bobSpring: false, gallop: 1 },
|
|
106
|
-
gallopBlend: [4.5, 9],
|
|
107
|
-
stance: { width: 1, gallopWidth: 0.55, sprawl: 0, crouch: 0, scuff: [12, 24, 0] },
|
|
108
|
-
head: {
|
|
109
|
-
carriage: [[0, 0.012, -0.012, 0], [9, -0.05, 0.04, 0.12]],
|
|
110
|
-
stab: [[0, 0], [0.9, 0.22], [1.8, 0.24], [4.5, 0.32], [9, 0.6]],
|
|
111
|
-
neckBend: 36, neckPivot: 0.6, yaw: 1.1, pitchUp: 0.5, pitchDown: 0.6, mouth: 'jawTip',
|
|
112
|
-
},
|
|
113
|
-
tail: { stiffness: 1, sway: 7, flick: 0, wag: 0, swish: 0, curl: 0, balance: 0, radius: [0.03, 0.015] },
|
|
114
|
-
ears: { rest: [0, 0], mobility: 1, speedFlatten: 0.5, twitch: 1, prick: 0.3 },
|
|
115
|
-
breath: { rate: 0.3, amp: 0.012, pant: false, pantRate: 2.4, pantAmp: 0.04, heatSpeed: 9 },
|
|
116
|
-
};
|
|
117
|
-
|
|
118
|
-
const _E = new THREE.Euler();
|
|
119
|
-
const FLATTEN_SPEEDS = [4, 14];
|
|
120
|
-
const LEG_KEYS = ['FL', 'FR', 'HL', 'HR'];
|
|
121
|
-
const LEN_KEYS = ['liftF', 'liftH', 'bobF', 'bobH', 'drop', 'lead'];
|
|
122
|
-
const GAIT_KEYS = ['f', 'D', 'liftF', 'liftH', 'bobF', 'bobH', 'flex', 'lat', 'fold', 'lead', 'heel', 'tail', 'drop', 'width'];
|
|
123
|
-
|
|
124
|
-
// mean fore / hind leg chain length (shoulder -> elbow -> wrist -> mcp, hip -> knee -> hock -> mtp) of
|
|
125
|
-
// a joint table ({ name: [x, y, z] }); the cheetah's reference skeleton gives CHEETAH_LEG
|
|
126
|
-
const CHEETAH_LEG = 0.5995;
|
|
127
|
-
function refLegLength(J) {
|
|
128
|
-
if (!J || !J.shoulderL || !J.mcpL || !J.hipL || !J.mtpL) return CHEETAH_LEG;
|
|
129
|
-
const d = (a, b) => Math.hypot(a[0] - b[0], a[1] - b[1], a[2] - b[2]);
|
|
130
|
-
const f = d(J.shoulderL, J.elbowL) + d(J.elbowL, J.wristL) + d(J.wristL, J.mcpL);
|
|
131
|
-
const h = d(J.hipL, J.kneeL) + d(J.kneeL, J.hockL) + d(J.hockL, J.mtpL);
|
|
132
|
-
return (f + h) / 2;
|
|
133
|
-
}
|
|
134
|
-
|
|
135
|
-
// ------------------------------------------------------------------------------------------------
|
|
136
|
-
// config: species data + joints -> numbers the engine uses (all lengths final, in metres)
|
|
137
|
-
// ------------------------------------------------------------------------------------------------
|
|
138
|
-
function resolveConfig(spec0, data) {
|
|
139
|
-
// per-variant overrides (species.motion.variants[params.variant]), merged one level deep, as in the
|
|
140
|
-
// other plans (ARCHITECTURE.md: "species.motion.variants[key] overrides motion data per variant")
|
|
141
|
-
const vo = spec0.variants?.[data.params?.variant];
|
|
142
|
-
const spec = vo ? { ...spec0 } : spec0;
|
|
143
|
-
if (vo) for (const [k, v] of Object.entries(vo)) spec[k] = v && typeof v === 'object' && !Array.isArray(v) && spec0[k] && typeof spec0[k] === 'object' && !Array.isArray(spec0[k]) ? { ...spec0[k], ...v } : v;
|
|
144
|
-
const J = {};
|
|
145
|
-
for (const [k, v] of Object.entries(data.joints)) J[k] = new THREE.Vector3(v[0], v[1], v[2]);
|
|
146
|
-
const s = data.params?.size || (spec.refHeight ? (J.scapTopL.y / spec.refHeight) : 1);
|
|
147
|
-
const sq = Math.sqrt(s);
|
|
148
|
-
// k: this individual's size relative to the cheetah reference that the engine's own absolute
|
|
149
|
-
// constants (clearances, re-step distances, posture offsets in metres, action timings) were tuned
|
|
150
|
-
// on. Species tables are in the species' own metres at its reference size and scale by s; engine
|
|
151
|
-
// constants scale by k = s * unit (lengths) and sqrt(k) (times, and speeds compared with engine
|
|
152
|
-
// speed constants). unit = the reference individual's leg length (shoulder -> mcp and hip -> mtp
|
|
153
|
-
// chains, from data.refJoints) / the cheetah's (CHEETAH_LEG); species.motion.unit overrides it.
|
|
154
|
-
const unit = spec.unit ?? refLegLength(data.refJoints) / CHEETAH_LEG, k = s * unit;
|
|
155
|
-
const merge = (a, b) => ({ ...a, ...(b || {}) });
|
|
156
|
-
const cfg = {
|
|
157
|
-
J, s, sq, unit, k, sk: Math.sqrt(k),
|
|
158
|
-
maxSpeed: (spec.maxSpeed ?? DEFAULTS.maxSpeed) * sq,
|
|
159
|
-
accel: spec.accel || DEFAULTS.accel,
|
|
160
|
-
decel: spec.decel ?? DEFAULTS.decel,
|
|
161
|
-
turnRate: (spec.turnRate ?? DEFAULTS.turnRate) / sq,
|
|
162
|
-
latAccelMax: spec.latAccelMax ?? DEFAULTS.latAccelMax,
|
|
163
|
-
moveCrouch: spec.moveCrouch ?? DEFAULTS.moveCrouch,
|
|
164
|
-
carpusLock: spec.carpusLock ?? DEFAULTS.carpusLock,
|
|
165
|
-
stride: merge(DEFAULTS.stride, spec.stride),
|
|
166
|
-
gears: spec.gears ? Object.fromEntries(Object.entries(spec.gears).map(([k, v]) => [k, v * sq])) : { walk: 1.2 * sq, trot: 3 * sq },
|
|
167
|
-
gallopBlend: spec.gallopBlend || DEFAULTS.gallopBlend,
|
|
168
|
-
flexBlend: spec.flexBlend || [3.5, 6],
|
|
169
|
-
gaits: (spec.gaits || []).map((g) => ({ f: 1, D: 0.6, liftF: 0.06, liftH: 0.06, bobF: 0, bobH: 0, flex: 0, lat: 0, fold: 1, lead: 0, heel: 30, tail: 0, drop: 0, width: 1, ...g })),
|
|
170
|
-
stance: merge(DEFAULTS.stance, spec.stance),
|
|
171
|
-
head: merge(DEFAULTS.head, spec.head),
|
|
172
|
-
tail: merge(DEFAULTS.tail, spec.tail),
|
|
173
|
-
ears: merge(DEFAULTS.ears, spec.ears),
|
|
174
|
-
breath: merge(DEFAULTS.breath, spec.breath),
|
|
175
|
-
actions: spec.actions || {},
|
|
176
|
-
reachDuty: spec.reachDuty ?? 1,
|
|
177
|
-
// Retrieviewer fork: opt-in smaller soft-IK zone for planted locomotion/idle feet.
|
|
178
|
-
// Keep upstream defaults for every other individual/action/swing.
|
|
179
|
-
stanceSoftMargin: Number.isFinite(data.params?.stanceSoftMargin) ? clamp(data.params.stanceSoftMargin, 0.005, 0.1) : null,
|
|
180
|
-
bankScale: spec.bankScale ?? 1,
|
|
181
|
-
};
|
|
182
|
-
if (!cfg.gaits.length) throw new Error('procedural-animals quadruped: species.motion.gaits is required');
|
|
183
|
-
// lateral spine bend a turn may take (total, pelvis -> chest, rad): in proportion to how far the
|
|
184
|
-
// spine flexes, read from the gait table's largest sagittal flexion (the cheetah's 0.56 rad gives
|
|
185
|
-
// the 0.45 rad it was tuned with; a horse's 0.13 rad gives 0.22)
|
|
186
|
-
cfg.bendMax = 0.45 * clamp(Math.sqrt(Math.max(0, ...cfg.gaits.map((g) => g.flex)) / 0.56), 0.45, 1.1);
|
|
187
|
-
// speed band (reference size) of every named gait: from its first to its last table row, so a
|
|
188
|
-
// forced gait (input.gait) reads a pure row of that gait, never a blend with its neighbours
|
|
189
|
-
cfg.gaitBands = {};
|
|
190
|
-
for (const r of cfg.gaits) { const b = cfg.gaitBands[r.name]; cfg.gaitBands[r.name] = b ? [b[0], r.v] : [r.v, r.v]; }
|
|
191
|
-
// ---- girdles and spine (bind)
|
|
192
|
-
cfg.zS = J.shoulderL.z; cfg.yS = J.shoulderL.y;
|
|
193
|
-
cfg.zH = J.hipL.z; cfg.yH = J.hipL.y;
|
|
194
|
-
const bodyDef = spec.body || {};
|
|
195
|
-
const sec = (d, gy, jx, jElbow) => ({
|
|
196
|
-
y: d?.y !== undefined ? d.y * s : gy - 0.25 * (gy - jElbow),
|
|
197
|
-
hh: d?.hh !== undefined ? d.hh * s : 0.35 * (gy - jElbow),
|
|
198
|
-
hw: d?.hw !== undefined ? d.hw * s : jx * 1.35,
|
|
199
|
-
});
|
|
200
|
-
cfg.chest = sec(bodyDef.chest, cfg.yS, J.shoulderL.x, J.elbowL.y);
|
|
201
|
-
cfg.pelvis = sec(bodyDef.pelvis, cfg.yH, J.hipL.x, J.kneeL.y);
|
|
202
|
-
// rump: the section behind the hips over the tail root (a sitting animal rests on it)
|
|
203
|
-
cfg.rump = bodyDef.rump ? { ...sec(bodyDef.rump, 0, 0, 0), z: bodyDef.rump.z * s }
|
|
204
|
-
: { y: cfg.pelvis.y, hh: cfg.pelvis.hh, hw: cfg.pelvis.hw * 0.8, z: lerp(cfg.zH, J.tailBase ? J.tailBase.z : cfg.zH, 0.85) };
|
|
205
|
-
cfg.mid = bodyDef.mid ? sec(bodyDef.mid, 0, 0, 0) : { y: lerp(cfg.chest.y, cfg.pelvis.y, 0.5), hh: lerp(cfg.chest.hh, cfg.pelvis.hh, 0.4), hw: lerp(cfg.chest.hw, cfg.pelvis.hw, 0.5) };
|
|
206
|
-
cfg.back = (bodyDef.back ?? 0.07) * s;
|
|
207
|
-
// the skin's own support around the torso sections (below and to either side), measured on the mesh:
|
|
208
|
-
// a lying or dead body rests on its skin, not only on the sections' nominal ellipses (a cow's belly
|
|
209
|
-
// hung 124 mm below its middle section and lay that far in the ground)
|
|
210
|
-
cfg.secZ = [cfg.zS, J.chestMid.z, J.thoraxRear.z, J.lumbosacral.z, cfg.zH, cfg.rump.z];
|
|
211
|
-
cfg.secSup = skinMemo(data, 'torso', () => torsoSupport(data, cfg.secZ, [cfg.chest, cfg.chest, cfg.mid, cfg.pelvis, cfg.pelvis, cfg.rump]));
|
|
212
|
-
// spine chain in the ground plane (x, z of the bind joints) for the girdles' offsets under a lateral
|
|
213
|
-
// bend (girdleBend): thorax rear -> chest middle -> shoulder centre; thorax rear -> lumbar middle ->
|
|
214
|
-
// lumbosacral -> hip centre
|
|
215
|
-
cfg.chain = new Float64Array([
|
|
216
|
-
J.chestMid.x - J.thoraxRear.x, J.chestMid.z - J.thoraxRear.z, -J.chestMid.x, cfg.zS - J.chestMid.z,
|
|
217
|
-
J.lumbarMid.x - J.thoraxRear.x, J.lumbarMid.z - J.thoraxRear.z, J.lumbosacral.x - J.lumbarMid.x, J.lumbosacral.z - J.lumbarMid.z,
|
|
218
|
-
-J.lumbosacral.x, cfg.zH - J.lumbosacral.z,
|
|
219
|
-
]);
|
|
220
|
-
// ---- legs
|
|
221
|
-
const feet = spec.feet || {};
|
|
222
|
-
cfg.legs = LEG_KEYS.map((key, idx) => {
|
|
223
|
-
const front = key[0] === 'F', S = key[1], side = S === 'L' ? 1 : -1;
|
|
224
|
-
const fd = feet[front ? 'front' : 'hind'] || {};
|
|
225
|
-
const type = fd.type || 'digitigrade';
|
|
226
|
-
const foot = { type, ...FOOT_DEFAULTS[type], ...fd };
|
|
227
|
-
const L = { key, idx, front, S, s: side, foot };
|
|
228
|
-
// precomputed names / joints (string concatenation in the hot path would allocate)
|
|
229
|
-
L.n = Object.fromEntries(['femur', 'tibia', 'metatarsus', 'hpaw', 'scapula', 'humerus', 'radius', 'metacarpus', 'fpaw'].map((b) => [b, b + S]));
|
|
230
|
-
L.jHip = J['hip' + S]; L.jShoulder = J['shoulder' + S]; L.jScapTop = J['scapTop' + S];
|
|
231
|
-
let back, M, T;
|
|
232
|
-
if (front) {
|
|
233
|
-
L.Lh = J['shoulder' + S].distanceTo(J['elbow' + S]);
|
|
234
|
-
L.Lr = J['elbow' + S].distanceTo(J['wrist' + S]);
|
|
235
|
-
L.Lmc = J['wrist' + S].distanceTo(J['mcp' + S]);
|
|
236
|
-
L.pivot = J['scapTop' + S].clone().lerp(J['shoulder' + S], 0.22);
|
|
237
|
-
L.len = L.Lh + L.Lr + L.Lmc;
|
|
238
|
-
M = J['mcp' + S]; T = J['ftoe' + S]; back = type === 'plantigrade' ? J['wrist' + S] : M;
|
|
239
|
-
// stance metacarpal tilt behind the normal (from the bind pose)
|
|
240
|
-
const d = J['wrist' + S].clone().sub(M);
|
|
241
|
-
L.a0 = foot.a0 !== undefined ? foot.a0 * DEG : Math.atan2(-d.z, d.y);
|
|
242
|
-
L.reach = foot.reach !== undefined ? foot.reach * L.len : (L.Lh + L.Lr) * 0.95 + L.Lmc + 0.04 * L.len;
|
|
243
|
-
} else {
|
|
244
|
-
const H = J['hip' + S], K = J['knee' + S], Hk = J['hock' + S], Mt = J['mtp' + S];
|
|
245
|
-
L.L1 = H.distanceTo(K); L.L2 = K.distanceTo(Hk); L.L3 = Hk.distanceTo(Mt);
|
|
246
|
-
const u0 = K.clone().sub(H).normalize(), m0 = Mt.clone().sub(Hk).normalize();
|
|
247
|
-
const e1 = Mt.clone().sub(H).normalize();
|
|
248
|
-
const e2 = new THREE.Vector3(0, 0, 1).addScaledVector(e1, -e1.z).normalize();
|
|
249
|
-
const n = new THREE.Vector3().crossVectors(e1, e2).normalize();
|
|
250
|
-
L.delta = Math.atan2(new THREE.Vector3().crossVectors(u0, m0).dot(n), u0.dot(m0));
|
|
251
|
-
const Wx = L.L1 + L.L3 * Math.cos(L.delta), Wy = L.L3 * Math.sin(L.delta);
|
|
252
|
-
L.Av = Math.hypot(Wx, Wy);
|
|
253
|
-
L.gamma = Math.atan2(Wy, Wx);
|
|
254
|
-
L.len = L.L1 + L.L2 + L.L3;
|
|
255
|
-
M = Mt; T = J['htoe' + S]; back = type === 'plantigrade' ? Hk : M;
|
|
256
|
-
L.reach = (foot.reach ?? 0.975) * (L.Av + L.L2);
|
|
257
|
-
}
|
|
258
|
-
// ground contact point (bind) and foot points relative to it
|
|
259
|
-
L.cz = lerp(back.z, T.z, foot.contact);
|
|
260
|
-
L.cx = Math.abs(M.x);
|
|
261
|
-
L.M = new THREE.Vector3(0, M.y, M.z - L.cz);
|
|
262
|
-
L.T = new THREE.Vector3(0, T.y, T.z - L.cz);
|
|
263
|
-
L.H = front ? null : new THREE.Vector3(0, J['hock' + S].y, J['hock' + S].z - L.cz);
|
|
264
|
-
// pitch of the metatarsus below the horizontal that plantRest keeps at rest: feet.hind.restTilt
|
|
265
|
-
// (deg), else the bind pose's
|
|
266
|
-
L.mtTilt = front ? 0 : foot.restTilt !== undefined ? foot.restTilt * DEG : Math.atan2(J['hock' + S].y - J['mtp' + S].y, J['mtp' + S].z - J['hock' + S].z);
|
|
267
|
-
L.plant = type === 'plantigrade';
|
|
268
|
-
// hooves (optional): a hoof bone (fhoof / hhoof: coffin joint -> toe) below the pastern (fpaw /
|
|
269
|
-
// hpaw: fetlock -> coffin joint). In stance the hoof stays flat while the fetlock sinks about the
|
|
270
|
-
// coffin joint; breakover and the swing flip rotate the whole digit about the toe.
|
|
271
|
-
const hoofName = (front ? 'fhoof' : 'hhoof') + S, Cj = J[(front ? 'fcoffin' : 'hcoffin') + S];
|
|
272
|
-
if (Cj && data.bones.some((b) => b.name === hoofName)) { L.hoof = hoofName; L.C = new THREE.Vector3(0, Cj.y, Cj.z - L.cz); }
|
|
273
|
-
L.leadMax = (foot.lead ?? (front ? 0.59 : 0.55)) * L.len;
|
|
274
|
-
L.behindMax = (foot.liftBehind ?? (front ? 0.88 : 0.79)) * L.len;
|
|
275
|
-
L.gz = front ? cfg.zS : cfg.zH;
|
|
276
|
-
L.gx = front ? J['shoulder' + S].x : J['hip' + S].x;
|
|
277
|
-
// skin thickness (bind) about the elbow / knee (rE: forearm / shank, the upper arm and thigh are in
|
|
278
|
-
// the body wall) and the wrist / hock (rW): how far above the ground a lying leg's joint centres
|
|
279
|
-
// stay (a bear's forearm is 10 cm thick at the elbow, and the fixed 5 % of the unit put its
|
|
280
|
-
// sleeping forearm 19 mm into the ground)
|
|
281
|
-
const r = skinMemo(data, 'leg' + key, () => (front ? {
|
|
282
|
-
rE: skinRadius(data, 'radius' + S, J['elbow' + S], J['wrist' + S], 0, 0.3),
|
|
283
|
-
rW: Math.max(skinRadius(data, 'radius' + S, J['elbow' + S], J['wrist' + S], 0.75, 1), skinRadius(data, 'metacarpus' + S, J['wrist' + S], J['mcp' + S], 0, 0.25)),
|
|
284
|
-
} : {
|
|
285
|
-
rE: skinRadius(data, 'tibia' + S, J['knee' + S], J['hock' + S], 0, 0.3),
|
|
286
|
-
rW: Math.max(skinRadius(data, 'tibia' + S, J['knee' + S], J['hock' + S], 0.75, 1), skinRadius(data, 'metatarsus' + S, J['hock' + S], J['mtp' + S], 0, 0.25)),
|
|
287
|
-
}));
|
|
288
|
-
L.rE = r.rE; L.rW = r.rW;
|
|
289
|
-
return L;
|
|
290
|
-
});
|
|
291
|
-
cfg.legLen = (cfg.legs[0].len + cfg.legs[2].len) * 0.5;
|
|
292
|
-
// ---- tail
|
|
293
|
-
cfg.tailN = 0;
|
|
294
|
-
while (J['tail' + (cfg.tailN + 1)] && data.bones.some((b) => b.name === 'tail' + cfg.tailN)) cfg.tailN++;
|
|
295
|
-
const n = cfg.tailN;
|
|
296
|
-
const resample = (arr) => Array.from({ length: n }, (_, i) => {
|
|
297
|
-
const x = n > 1 ? (i / (n - 1)) * (arr.length - 1) : 0;
|
|
298
|
-
const a = Math.floor(x), b = Math.min(arr.length - 1, a + 1);
|
|
299
|
-
return lerp(arr[a], arr[b], x - a) * DEG;
|
|
300
|
-
});
|
|
301
|
-
if (n) {
|
|
302
|
-
const bindPitch = Array.from({ length: n }, (_, i) => {
|
|
303
|
-
const d = J['tail' + (i + 1)].clone().sub(J['tail' + i]);
|
|
304
|
-
return Math.atan2(d.y, -d.z) / DEG;
|
|
305
|
-
});
|
|
306
|
-
const rows = cfg.tail.carriage || [[0, bindPitch], [3, bindPitch.map((p) => p * 0.6)], [10, bindPitch.map((p) => p * 0.3)]];
|
|
307
|
-
cfg.tailCarry = rows.map(([v, arr]) => [v, ...resample(arr)]);
|
|
308
|
-
}
|
|
309
|
-
// ---- head
|
|
310
|
-
cfg.headCarry = cfg.head.carriage.map((r) => [r[0], r[1] * s, r[2] * s, r[3]]);
|
|
311
|
-
cfg.headStab = cfg.head.stab;
|
|
312
|
-
// stride nod [walk, trot, gallop, phase shift] (share of the neck length): hoofed animals nod
|
|
313
|
-
// clearly at the walk (a horse ~10 cm) and pump the neck at the gallop; paws carry the head
|
|
314
|
-
// steadier (a cat's hardly moves)
|
|
315
|
-
if (!cfg.head.nod) cfg.head.nod = (spec.feet?.front?.type || 'digitigrade') === 'unguligrade' ? [0.11, 0.05, 0.12, 0.04] : [0.035, 0.025, 0.05, 0.04];
|
|
316
|
-
const mouthJ = J[cfg.head.mouth] || J.jawTip || J.nose;
|
|
317
|
-
cfg.mouthLocal = mouthJ.clone().lerp(J.nose, 0.35).sub(J.occiput); // bind, relative to the occiput
|
|
318
|
-
cfg.noseLocal = J.nose.clone().sub(J.occiput);
|
|
319
|
-
// (the chin, for the head's ground clearance: the lowest skin of the jaw, else the jaw's tip, else
|
|
320
|
-
// the mouth point: a cow's jaw reaches 5 cm below its tip joint, and a dead cow's jaw went 38 mm
|
|
321
|
-
// into the ground)
|
|
322
|
-
cfg.chinLocal = skinMemo(data, 'chin', () => lowestSkin(data, 'jaw', J.occiput)) || (J.jawTip ? J.jawTip.clone().sub(J.occiput) : cfg.mouthLocal);
|
|
323
|
-
cfg.earLocal = J.earBaseL && J.earBaseR ? [J.earBaseL.clone().sub(J.occiput), J.earBaseR.clone().sub(J.occiput)] : null;
|
|
324
|
-
// the head's widest points, left and right (horns, antlers or cheeks: skin on the head bone), bind,
|
|
325
|
-
// relative to the occiput: a head lying on its side keeps them out of the ground like the ear bases
|
|
326
|
-
// (a dead highland cow's horn went 0.31 m into it)
|
|
327
|
-
cfg.headSide = skinMemo(data, 'headSide', () => headSides(data, J.occiput));
|
|
328
|
-
cfg.headSideReach = cfg.headSide ? Math.max(...cfg.headSide.flat().map((p) => p.length())) : 0;
|
|
329
|
-
cfg.neckLen = J.neckBase.distanceTo(J.neckMid) + J.neckMid.distanceTo(J.occiput);
|
|
330
|
-
cfg.hasJaw = data.bones.some((b) => b.name === 'jaw');
|
|
331
|
-
cfg.hasEars = data.bones.some((b) => b.name === 'earL');
|
|
332
|
-
return cfg;
|
|
333
|
-
}
|
|
334
|
-
|
|
335
|
-
// Skin measurements of a built mesh (pure functions of its data: every animal built from it shares
|
|
336
|
-
// them, read only; a hero mesh has 140k vertices)
|
|
337
|
-
const SKIN = new WeakMap();
|
|
338
|
-
function skinMemo(data, key, fn) {
|
|
339
|
-
let m = SKIN.get(data);
|
|
340
|
-
if (!m) SKIN.set(data, (m = new Map()));
|
|
341
|
-
if (!m.has(key)) m.set(key, fn());
|
|
342
|
-
return m.get(key);
|
|
343
|
-
}
|
|
344
|
-
|
|
345
|
-
// Support of the torso's skin about each section (bind centre (0, sec.y, z)): for directions
|
|
346
|
-
// d = (sin th, -cos th, 0), th = -90 .. 90 deg in SUP_N steps (straight down at the middle), the largest
|
|
347
|
-
// (p - c) . d over the visible skin carried mostly by the torso bones, each vertex counted at the
|
|
348
|
-
// section nearest to it along the body
|
|
349
|
-
const SUP_N = 13, TORSO_RE = /^(spine[123]|chest|pelvis)$/;
|
|
350
|
-
function torsoSupport(data, zs, secs) {
|
|
351
|
-
const bones = data.bones, P = data.pos, SI = data.skinIndex, SW = data.skinWeight, coat = data.coat;
|
|
352
|
-
if (!bones || !P || !SI || !SW) return null;
|
|
353
|
-
const isT = Uint8Array.from(bones, (b) => (TORSO_RE.test(b.name) ? 1 : 0));
|
|
354
|
-
const sup = zs.map(() => new Float64Array(SUP_N).fill(-Infinity));
|
|
355
|
-
const sn = Float64Array.from({ length: SUP_N }, (_, j) => Math.sin((j / (SUP_N - 1) - 0.5) * Math.PI));
|
|
356
|
-
const cs = Float64Array.from({ length: SUP_N }, (_, j) => Math.cos((j / (SUP_N - 1) - 0.5) * Math.PI));
|
|
357
|
-
for (let v = 0, nv = P.length / 3; v < nv; v++) {
|
|
358
|
-
if (coat && coat[v * 4 + 3] < 0.5) continue;
|
|
359
|
-
let w = 0;
|
|
360
|
-
for (let k = 0; k < 4; k++) if (isT[SI[v * 4 + k]]) w += SW[v * 4 + k];
|
|
361
|
-
if (w < 0.5) continue;
|
|
362
|
-
const x = P[v * 3], y = P[v * 3 + 1], z = P[v * 3 + 2];
|
|
363
|
-
let kb = 0;
|
|
364
|
-
for (let k = 1; k < zs.length; k++) if (Math.abs(z - zs[k]) < Math.abs(z - zs[kb])) kb = k;
|
|
365
|
-
const T = sup[kb], dy = y - secs[kb].y;
|
|
366
|
-
for (let j = 0; j < SUP_N; j++) { const h = x * sn[j] - dy * cs[j]; if (h > T[j]) T[j] = h; }
|
|
367
|
-
}
|
|
368
|
-
return sup;
|
|
369
|
-
}
|
|
370
|
-
|
|
371
|
-
const EAR_PTS = 13;
|
|
372
|
-
// an ear's skin extremes (bind, relative to its base B; visible skin of weight >= 0.5, beyond 0.3 of
|
|
373
|
-
// the ear's length): the farthest along the ear, and the farthest to either side across it and front
|
|
374
|
-
// / back; reach: the largest distance of them from the base
|
|
375
|
-
function earRim(data, bone, B, T) {
|
|
376
|
-
const bi = data.bones ? data.bones.findIndex((b) => b.name === bone) : -1;
|
|
377
|
-
const P = data.pos, SI = data.skinIndex, SW = data.skinWeight, coat = data.coat;
|
|
378
|
-
if (bi < 0 || !P || !SI || !SW) return { pts: null, reach: 0 };
|
|
379
|
-
const a = new THREE.Vector3().subVectors(T, B), L = a.length();
|
|
380
|
-
if (L < 1e-9) return { pts: null, reach: 0 };
|
|
381
|
-
a.multiplyScalar(1 / L);
|
|
382
|
-
const u = new THREE.Vector3(1, 0, 0).addScaledVector(a, -a.x);
|
|
383
|
-
if (u.lengthSq() < 1e-6) u.set(0, 0, 1).addScaledVector(a, -a.z);
|
|
384
|
-
u.normalize();
|
|
385
|
-
const w = new THREE.Vector3().crossVectors(a, u);
|
|
386
|
-
// (along the ear, 8 ways across it, and 4 half way between)
|
|
387
|
-
const dirs = [a];
|
|
388
|
-
for (let j = 0; j < 8; j++) dirs.push(u.clone().multiplyScalar(Math.cos(j * Math.PI / 4)).addScaledVector(w, Math.sin(j * Math.PI / 4)));
|
|
389
|
-
for (let j = 0; j < 4; j++) dirs.push(u.clone().multiplyScalar(Math.cos(j * Math.PI / 2)).addScaledVector(w, Math.sin(j * Math.PI / 2)).add(a).normalize());
|
|
390
|
-
const best = dirs.map(() => -Infinity), pts = dirs.map(() => null);
|
|
391
|
-
const d = new THREE.Vector3();
|
|
392
|
-
for (let v = 0, n = P.length / 3; v < n; v++) {
|
|
393
|
-
if (coat && coat[v * 4 + 3] < 0.5) continue;
|
|
394
|
-
let wt = 0;
|
|
395
|
-
for (let k = 0; k < 4; k++) if (SI[v * 4 + k] === bi) wt += SW[v * 4 + k];
|
|
396
|
-
if (wt < 0.5) continue;
|
|
397
|
-
d.set(P[v * 3] - B.x, P[v * 3 + 1] - B.y, P[v * 3 + 2] - B.z);
|
|
398
|
-
if (d.dot(a) < 0.3 * L) continue;
|
|
399
|
-
for (let j = 0; j < dirs.length; j++) { const x = d.dot(dirs[j]); if (x > best[j]) { best[j] = x; pts[j] = d.clone(); } }
|
|
400
|
-
}
|
|
401
|
-
if (pts.some((p) => !p)) return { pts: null, reach: 0 };
|
|
402
|
-
return { pts, reach: Math.max(...pts.map((p) => p.length())) };
|
|
403
|
-
}
|
|
404
|
-
|
|
405
|
-
// largest distance (bind) of a bone's visible skin (weight >= 0.5) from its axis A -> B, over the part
|
|
406
|
-
// of the bone between t0 and t1 (0: A, 1: B)
|
|
407
|
-
function skinRadius(data, bone, A, B, t0, t1) {
|
|
408
|
-
const bi = data.bones ? data.bones.findIndex((b) => b.name === bone) : -1;
|
|
409
|
-
const P = data.pos, SI = data.skinIndex, SW = data.skinWeight, coat = data.coat;
|
|
410
|
-
if (bi < 0 || !P || !SI || !SW || !A || !B) return 0;
|
|
411
|
-
const ax = B.x - A.x, ay = B.y - A.y, az = B.z - A.z, l2 = ax * ax + ay * ay + az * az;
|
|
412
|
-
if (l2 < 1e-12) return 0;
|
|
413
|
-
let r = 0;
|
|
414
|
-
for (let v = 0, n = P.length / 3; v < n; v++) {
|
|
415
|
-
if (coat && coat[v * 4 + 3] < 0.5) continue;
|
|
416
|
-
let w = 0;
|
|
417
|
-
for (let k = 0; k < 4; k++) if (SI[v * 4 + k] === bi) w += SW[v * 4 + k];
|
|
418
|
-
if (w < 0.5) continue;
|
|
419
|
-
const px = P[v * 3] - A.x, py = P[v * 3 + 1] - A.y, pz = P[v * 3 + 2] - A.z;
|
|
420
|
-
const t = (px * ax + py * ay + pz * az) / l2;
|
|
421
|
-
if (t < t0 || t > t1) continue;
|
|
422
|
-
r = Math.max(r, Math.hypot(px - t * ax, py - t * ay, pz - t * az));
|
|
423
|
-
}
|
|
424
|
-
return r;
|
|
425
|
-
}
|
|
426
|
-
|
|
427
|
-
// the lowest skin point (bind) of a bone (weight >= 0.9), relative to O
|
|
428
|
-
function lowestSkin(data, bone, O) {
|
|
429
|
-
const bi = data.bones ? data.bones.findIndex((b) => b.name === bone) : -1;
|
|
430
|
-
const P = data.pos, SI = data.skinIndex, SW = data.skinWeight;
|
|
431
|
-
if (bi < 0 || !P || !SI || !SW || !O) return null;
|
|
432
|
-
let best = null, by = Infinity;
|
|
433
|
-
for (let v = 0, n = P.length / 3; v < n; v++) {
|
|
434
|
-
let w = 0;
|
|
435
|
-
for (let k = 0; k < 4; k++) if (SI[v * 4 + k] === bi) w += SW[v * 4 + k];
|
|
436
|
-
if (w < 0.9 || P[v * 3 + 1] >= by) continue;
|
|
437
|
-
by = P[v * 3 + 1]; best = new THREE.Vector3(P[v * 3] - O.x, P[v * 3 + 1] - O.y, P[v * 3 + 2] - O.z);
|
|
438
|
-
}
|
|
439
|
-
return best;
|
|
440
|
-
}
|
|
441
|
-
|
|
442
|
-
// The skin of the head and jaw that a head lying on its side rests on: for either side, the skin point
|
|
443
|
-
// farthest along each of the directions d = (+-sin th, -cos th, 0), th = 60, 90 deg (below and aside,
|
|
444
|
-
// and straight out: jaw / cheek, horn / antler), bind, relative to the occiput O ([left, right]).
|
|
445
|
-
// (With 45 and 75 deg too a highland cow's jaw and horn points could not all be cleared by a roll and
|
|
446
|
-
// its head fell 20 cm into the ground; with only the widest point a hereford's jaw went 34 mm in)
|
|
447
|
-
const SIDE_TH = [60, 90].map((d) => d * DEG);
|
|
448
|
-
function headSides(data, O) {
|
|
449
|
-
const bones = data.bones, P = data.pos, SI = data.skinIndex, SW = data.skinWeight, coat = data.coat;
|
|
450
|
-
if (!bones || !P || !SI || !SW || !O) return null;
|
|
451
|
-
const isH = Uint8Array.from(bones, (b) => (b.name === 'head' || b.name === 'jaw' ? 1 : 0));
|
|
452
|
-
const out = [0, 1].map(() => SIDE_TH.map(() => ({ h: -Infinity, v: -1 })));
|
|
453
|
-
for (let v = 0, n = P.length / 3; v < n; v++) {
|
|
454
|
-
if (coat && coat[v * 4 + 3] < 0.5) continue;
|
|
455
|
-
let w = 0;
|
|
456
|
-
for (let k = 0; k < 4; k++) if (isH[SI[v * 4 + k]]) w += SW[v * 4 + k];
|
|
457
|
-
if (w < 0.5) continue;
|
|
458
|
-
const x = P[v * 3] - O.x, y = P[v * 3 + 1] - O.y;
|
|
459
|
-
for (let s = 0; s < 2; s++) for (let j = 0; j < SIDE_TH.length; j++) {
|
|
460
|
-
const h = (s === 0 ? x : -x) * Math.sin(SIDE_TH[j]) - y * Math.cos(SIDE_TH[j]);
|
|
461
|
-
if (h > out[s][j].h) { out[s][j].h = h; out[s][j].v = v; }
|
|
462
|
-
}
|
|
463
|
-
}
|
|
464
|
-
if (out.some((side) => side.some((e) => e.v < 0))) return null;
|
|
465
|
-
return out.map((side) => side.map((e) => new THREE.Vector3(P[e.v * 3] - O.x, P[e.v * 3 + 1] - O.y, P[e.v * 3 + 2] - O.z)));
|
|
466
|
-
}
|
|
467
|
-
|
|
468
|
-
// ------------------------------------------------------------------------------------------------
|
|
469
|
-
// module-level helpers (no closures in the hot paths: they would allocate every call)
|
|
470
|
-
function restHeight(sec, gy, dl) {
|
|
471
|
-
// girdle joint height above the ground when the torso section rests on it (dl: world down in body frame)
|
|
472
|
-
const sup = Math.sqrt((sec.hh * dl.y) ** 2 + (sec.hw * dl.x) ** 2 + (0.3 * sec.hh * dl.z) ** 2);
|
|
473
|
-
return sup - (sec.y - gy) * (-dl.y);
|
|
474
|
-
}
|
|
475
|
-
function footPoint(out, c, latA, n, fwd, loc) {
|
|
476
|
-
return out.copy(c).addScaledVector(latA, loc.x).addScaledVector(n, loc.y).addScaledVector(fwd, loc.z);
|
|
477
|
-
}
|
|
478
|
-
// Digit placement for a heel / hoof angle beta: beta >= 0 rolls the foot (M, and the coffin joint C
|
|
479
|
-
// of a hoof) up about the toe T; beta < 0 with a coffin joint sinks the fetlock M about C (hoof flat)
|
|
480
|
-
function placeDigit(beta, Mflat, Cf, T, latA, M, C) {
|
|
481
|
-
const q = tqa(latA, beta);
|
|
482
|
-
if (beta >= 0 || !Cf) {
|
|
483
|
-
M.copy(Mflat).sub(T).applyQuaternion(q).add(T);
|
|
484
|
-
if (C && Cf) C.copy(Cf).sub(T).applyQuaternion(q).add(T);
|
|
485
|
-
} else {
|
|
486
|
-
if (C) C.copy(Cf);
|
|
487
|
-
M.copy(Mflat).sub(Cf).applyQuaternion(q).add(Cf);
|
|
488
|
-
}
|
|
489
|
-
}
|
|
490
|
-
// Best-fit scapula protraction: minimise (reach excess)^2 + k (pr - prot0)^2 by golden-section
|
|
491
|
-
// search. The optimum moves continuously with the target, so the shoulder cannot jump between frames
|
|
492
|
-
// even when the target is out of reach. All scalar math over a Float64Array (no boxing).
|
|
493
|
-
const SC = new Float64Array(12);
|
|
494
|
-
function scapBestFit(sc, iters) {
|
|
495
|
-
// (cost evaluated inline: a call returning a double would box it)
|
|
496
|
-
const ax = sc[0], ay = sc[1], az = sc[2], bx = sc[3], by = sc[4], bz = sc[5];
|
|
497
|
-
const wx = sc[6], wy = sc[7], wz = sc[8], lim = sc[9], p0 = sc[10], k = sc[11];
|
|
498
|
-
let lo = -0.8, hi = 0.6, f1 = 0, f2 = 0;
|
|
499
|
-
const gr = 0.6180339887;
|
|
500
|
-
let x1 = hi - gr * (hi - lo), x2 = lo + gr * (hi - lo);
|
|
501
|
-
for (let it = -2; it < iters; it++) {
|
|
502
|
-
// it = -2 / -1: initial evaluations of x1 / x2
|
|
503
|
-
// new probe: shrink [lo, hi] to [lo, x2] (probe below x1) or [x1, hi] (probe above x2)
|
|
504
|
-
const x = it === -2 ? x1 : it === -1 ? x2 : f1 < f2 ? x2 - gr * (x2 - lo) : x1 + gr * (hi - x1);
|
|
505
|
-
const co = Math.cos(x), si = Math.sin(x);
|
|
506
|
-
const dx = ax * co + bx * si - wx, dy = ay * co + by * si - wy, dz = az * co + bz * si - wz;
|
|
507
|
-
const e = Math.max(0, Math.sqrt(dx * dx + dy * dy + dz * dz) - lim);
|
|
508
|
-
const f = e * e + k * (x - p0) * (x - p0);
|
|
509
|
-
if (it === -2) f1 = f;
|
|
510
|
-
else if (it === -1) f2 = f;
|
|
511
|
-
else if (f1 < f2) { hi = x2; x2 = x1; f2 = f1; x1 = x; f1 = f; }
|
|
512
|
-
else { lo = x1; x1 = x2; f1 = f2; x2 = x; f2 = f; }
|
|
513
|
-
}
|
|
514
|
-
return 0.5 * (lo + hi);
|
|
515
|
-
}
|
|
516
|
-
function arcPoint(out, base, tB, nC, nCl, th, Ln, sArc) {
|
|
517
|
-
if (th < 1e-4 || nCl < 1e-7) return out.copy(base).addScaledVector(tB, sArc);
|
|
518
|
-
const R = Ln / (2 * th), a = sArc / R;
|
|
519
|
-
return out.copy(base).addScaledVector(tB, R * Math.sin(a)).addScaledVector(nC, (R * (1 - Math.cos(a))) / nCl);
|
|
520
|
-
}
|
|
521
|
-
const expTo = (x, target, dt, tau) => x + (target - x) * (1 - Math.exp(-dt / tau));
|
|
522
|
-
// sqrt(max(0, u)) made well conditioned near u = 0: exact from u0 up; below it the cubic from 0 (zero
|
|
523
|
-
// slope) at u = -u0 to sqrt(u0) with the root's own slope at u = u0 (C1, slope <= 0.5 / sqrt(u0),
|
|
524
|
-
// within 0.375 sqrt(u0) of the root)
|
|
525
|
-
function softRoot(u, u0) {
|
|
526
|
-
if (u >= u0) return Math.sqrt(u);
|
|
527
|
-
if (u <= -u0) return 0;
|
|
528
|
-
const t = (u + u0) / (2 * u0);
|
|
529
|
-
return Math.sqrt(u0) * t * t * (2 - t);
|
|
530
|
-
}
|
|
531
|
-
// Tail spring chain: each segment is a damped angular spring toward a target bend relative to its
|
|
532
|
-
// parent (blended with an absolute target by wAbs), integrated semi-implicitly in sub steps.
|
|
533
|
-
const _tq = new THREE.Quaternion(), _ta = new THREE.Vector3(), _tt = new THREE.Vector3();
|
|
534
|
-
function integrateTail(D, Wv, U, R, n, sub, h, stiffen, wAbs) {
|
|
535
|
-
const q = _tq, axis = _ta, tgt = _tt, z = 0.72;
|
|
536
|
-
for (let st = 0; st < sub; st++) {
|
|
537
|
-
for (let i = 0; i < n; i++) {
|
|
538
|
-
if (i === 0) tgt.copy(U[0]);
|
|
539
|
-
else tgt.copy(D[i - 1]).applyQuaternion(R[i]).lerp(U[i], wAbs).normalize();
|
|
540
|
-
const d = D[i], w = Wv[i];
|
|
541
|
-
const cosA = clamp(d.x * tgt.x + d.y * tgt.y + d.z * tgt.z, -1, 1);
|
|
542
|
-
axis.crossVectors(d, tgt);
|
|
543
|
-
const sn = Math.sqrt(axis.x * axis.x + axis.y * axis.y + axis.z * axis.z);
|
|
544
|
-
const ang = Math.atan2(sn, cosA);
|
|
545
|
-
const k = sn > 1e-8 ? ang / sn : 0;
|
|
546
|
-
const om = (15 + (9 - 15) * (i / Math.max(1, n - 1))) * stiffen;
|
|
547
|
-
const a = om * om * h * k, damp = 1 / (1 + 2 * z * om * h);
|
|
548
|
-
w.x = (w.x + axis.x * a) * damp; w.y = (w.y + axis.y * a) * damp; w.z = (w.z + axis.z * a) * damp;
|
|
549
|
-
const wd = w.x * d.x + w.y * d.y + w.z * d.z; // drop spin about the segment's own axis
|
|
550
|
-
w.x -= d.x * wd; w.y -= d.y * wd; w.z -= d.z * wd;
|
|
551
|
-
const wl = Math.sqrt(w.x * w.x + w.y * w.y + w.z * w.z);
|
|
552
|
-
if (wl > 1e-9) { axis.set(w.x / wl, w.y / wl, w.z / wl); q.setFromAxisAngle(axis, wl * h); d.applyQuaternion(q).normalize(); }
|
|
553
|
-
}
|
|
554
|
-
}
|
|
555
|
-
}
|
|
556
|
-
function tailDir(out, back, up, lat, p, y) {
|
|
557
|
-
const cp = Math.cos(p);
|
|
558
|
-
return out.copy(back).multiplyScalar(cp * Math.cos(y)).addScaledVector(up, Math.sin(p)).addScaledVector(lat, cp * Math.sin(y)).normalize();
|
|
559
|
-
}
|
|
560
|
-
|
|
561
|
-
// ------------------------------------------------------------------------------------------------
|
|
562
|
-
export function createQuadrupedMotion(ctx) { return new QuadrupedMotion(ctx); }
|
|
563
|
-
|
|
564
|
-
export class QuadrupedMotion {
|
|
565
|
-
constructor({ species, data, skeleton, ground, water, position, heading = 0, emit }) {
|
|
566
|
-
this.species = species;
|
|
567
|
-
this.cfg = resolveConfig(species.motion || {}, data);
|
|
568
|
-
const cfg = this.cfg;
|
|
569
|
-
this.sk = skeleton;
|
|
570
|
-
this.ground = ground || (() => 0);
|
|
571
|
-
this.water = water || null;
|
|
572
|
-
this.emit = emit || (() => {});
|
|
573
|
-
this.rand = prng((data.seed || 1) * 7919 + 13);
|
|
574
|
-
// stride-to-stride variation (its own stream, so the tail's idle randomness keeps its sequence):
|
|
575
|
-
// [frequency, front lift, hind lift, bounce], each from the last stride's value to the next one's
|
|
576
|
-
this.randS = prng((data.seed || 1) * 104729 + 7);
|
|
577
|
-
this.sv = { a: [0, 0, 0, 0], b: [0, 0, 0, 0], x: [0, 0, 0, 0] };
|
|
578
|
-
// bone lookups
|
|
579
|
-
this.bone = {};
|
|
580
|
-
this.bindQ = {};
|
|
581
|
-
for (let i = 0; i < data.bones.length; i++) {
|
|
582
|
-
const name = data.bones[i].name;
|
|
583
|
-
this.bone[name] = skeleton.bones[i];
|
|
584
|
-
this.bindQ[name] = new THREE.Quaternion().setFromRotationMatrix(skeleton.bind[i]);
|
|
585
|
-
}
|
|
586
|
-
const J = cfg.J;
|
|
587
|
-
this.J = J;
|
|
588
|
-
// ---- public contract
|
|
589
|
-
// (climb: part of the common motion contract; the quadruped plan follows the terrain and ignores it)
|
|
590
|
-
this.input = { speed: 0, heading, climb: 0, look: null, follow: null, target: null, targetSpeed: 3, crouch: 0, gait: null };
|
|
591
|
-
this.pos = new THREE.Vector3();
|
|
592
|
-
this.velocity = new THREE.Vector3();
|
|
593
|
-
this.heading = heading;
|
|
594
|
-
this.speed = 0;
|
|
595
|
-
this.yawRate = 0;
|
|
596
|
-
this.yawAcc = 0;
|
|
597
|
-
// crab angle: direction of the root's motion relative to the heading. Zero when the engine steers
|
|
598
|
-
// itself; in follow mode an external body may carry the animal sideways (a knock, a sharp turn of
|
|
599
|
-
// the followed body) until the heading has caught up, and the feet are placed for the motion the
|
|
600
|
-
// body actually makes (along the heading, a landing forefoot was left out of reach sideways)
|
|
601
|
-
this.crab = 0;
|
|
602
|
-
this.phase = 0;
|
|
603
|
-
this.brake = 0;
|
|
604
|
-
this.time = 0;
|
|
605
|
-
this.heat = 0;
|
|
606
|
-
this.lod = 0;
|
|
607
|
-
this.frame = 0;
|
|
608
|
-
this.frameDt = 1 / 60;
|
|
609
|
-
this.lookTarget = new THREE.Vector3();
|
|
610
|
-
this.state = {
|
|
611
|
-
position: this.pos, heading, speed: 0, velocity: this.velocity, gait: 'stand', grounded: true,
|
|
612
|
-
action: null, posture: 'stand', eyelid: 0, lookTarget: null,
|
|
613
|
-
stats: { gait: 'stand', freq: 0, duty: 1 },
|
|
614
|
-
legs: LEG_KEYS.map((key) => ({ key, stance: true, contact: new THREE.Vector3() })),
|
|
615
|
-
};
|
|
616
|
-
this.headPose = { position: new THREE.Vector3(), quaternion: new THREE.Quaternion() };
|
|
617
|
-
this.debug = { targets: Array.from({ length: 8 }, () => new THREE.Vector3()) };
|
|
618
|
-
this.gaits = [...new Set(cfg.gaits.map((g) => g.name))];
|
|
619
|
-
this.gears = cfg.gears;
|
|
620
|
-
// ---- gait sample (reused)
|
|
621
|
-
this.gait = { name: 'walk', blend: 0, gallop: 0, off: [0, 0, 0, 0], DL: [0.6, 0.6, 0.6, 0.6] };
|
|
622
|
-
for (const k of GAIT_KEYS) this.gait[k] = 0;
|
|
623
|
-
// ---- springs
|
|
624
|
-
this.bank = new Spring(0); this.pitch = new Spring(0); this.height = new Spring(0);
|
|
625
|
-
this.hipY = new Spring(0); this.shY = new Spring(0);
|
|
626
|
-
this.flexS = new Spring(0); this.latS = new Spring(0); this.latH = new Spring(0);
|
|
627
|
-
this._latF = 0; this._latH = 0; // the locomotion's lateral spine bend, last pose (front / hind, see buildBody)
|
|
628
|
-
this._poseH = heading; // heading of the last pose
|
|
629
|
-
this._gb = new Float64Array(3); this._fl = new Float64Array(2);
|
|
630
|
-
this.lookYaw = new Spring(0); this.headPitch = new Spring(0); this.headRollS = new Spring(0);
|
|
631
|
-
this.earL = new Spring(0); this.earR = new Spring(0); this.earSwL = new Spring(0); this.earSwR = new Spring(0);
|
|
632
|
-
this.jaw = new Spring(0);
|
|
633
|
-
this.hx = new Spring(0); this.hy = new Spring(0); this.hz = new Spring(0);
|
|
634
|
-
this.speedS = new Spring(0);
|
|
635
|
-
this.breath = 0;
|
|
636
|
-
this.accel = new THREE.Vector3();
|
|
637
|
-
this.lastVel = new THREE.Vector3();
|
|
638
|
-
this.push = new THREE.Vector3(); // external shove (hit stagger), decays
|
|
639
|
-
this.lift = 0; // vertical body offset (jump flight), set by the action layer
|
|
640
|
-
this.speedOverride = null; // ground speed imposed by an action (jump push-off)
|
|
641
|
-
// airborne state (jump)
|
|
642
|
-
this.air = { active: false, t: 0, vy: 0, y0: 0, vh: 0, T: 0 };
|
|
643
|
-
// landing sag of the fore / hind girdles after a touchdown (see touchDown, pose)
|
|
644
|
-
this.sag = { on: false, F: new Spring(0), H: new Spring(0), wF: 17, wH: 17 };
|
|
645
|
-
// ---- legs
|
|
646
|
-
this.legs = cfg.legs.map((L) => ({
|
|
647
|
-
def: L, state: 'stance',
|
|
648
|
-
plant: new THREE.Vector3(), plantN: new THREE.Vector3(0, 1, 0),
|
|
649
|
-
liftLocal: new THREE.Vector3(), liftN: new THREE.Vector3(0, 1, 0),
|
|
650
|
-
land: new THREE.Vector3(), landLocal: new THREE.Vector3(), contact: new THREE.Vector3(),
|
|
651
|
-
swingT: 0, swingDur: 0.3, swingNom: 0.3, stanceTime: 0, prevPhase: 0, windowUsed: false,
|
|
652
|
-
u: 0, w: 0, heelLift: 0, betaLast: 0, betaStart: 0, betaAdd: 0, kStart: -L.a0, protStart: 0, liftH: 0.05, mode: 'gait', track: false, uPace: 3, wPace: 3, betaHeel: 0, swingAmp: 1, bend: [new THREE.Vector3(), new THREE.Vector3()], bendInit: [false, false], plantFwd: new THREE.Vector3(0, 0, 1), fwdInit: false, overreach: 0, ovRate: 0, reachF: 1, reachB: -1, reachBL: -1, sweep: 0,
|
|
653
|
-
idleOff: new THREE.Vector3(),
|
|
654
|
-
// last IK solve: girdle joint and the targets of its stages with their lengths (predicted lift-off)
|
|
655
|
-
ikJ: new THREE.Vector3(), ikT1: new THREE.Vector3(), ikT2: new THREE.Vector3(), ikM1: 0, ikM2: 0,
|
|
656
|
-
protS: new Spring(0), protInit: false, toeS: new Spring(0), strS: new Spring(0), sinkS: new Spring(0), sinkLift: 0,
|
|
657
|
-
// solved joint positions (world) for LOD 2 extrapolation
|
|
658
|
-
jp: Array.from({ length: 7 }, () => new THREE.Vector3()),
|
|
659
|
-
jp0: Array.from({ length: 7 }, () => new THREE.Vector3()),
|
|
660
|
-
jpp: Array.from({ length: 7 }, () => new THREE.Vector3()), sf: [-9, -9, -9],
|
|
661
|
-
solved: 0,
|
|
662
|
-
// blended foot target
|
|
663
|
-
ft: { c: new THREE.Vector3(), n: new THREE.Vector3(0, 1, 0), fwd: new THREE.Vector3(0, 0, 1), beta: 0, k: 0, dirW: 0, dir: new THREE.Vector3(0, 0, 1), free: 0 },
|
|
664
|
-
}));
|
|
665
|
-
// ---- tail
|
|
666
|
-
const n = cfg.tailN;
|
|
667
|
-
this.tail = {
|
|
668
|
-
n, p: Array.from({ length: n + 1 }, () => new THREE.Vector3()),
|
|
669
|
-
len: Array.from({ length: n }, (_, i) => J['tail' + i].distanceTo(J['tail' + (i + 1)])),
|
|
670
|
-
D: Array.from({ length: n }, () => new THREE.Vector3()), W: Array.from({ length: n }, () => new THREE.Vector3()),
|
|
671
|
-
U: Array.from({ length: n }, () => new THREE.Vector3()), rel: Array.from({ length: n }, () => new THREE.Quaternion()),
|
|
672
|
-
pitch: new Float64Array(n), yaw: new Float64Array(n), carry: new Array(n).fill(0),
|
|
673
|
-
init: false, prevV: 0, aFwdS: 0, aLatFast: 0, aLatSlow: 0,
|
|
674
|
-
flickT: 3, flickAmp: 0, flickAge: 10, swishT: 2, swishAmp: 0, swishAge: 10,
|
|
675
|
-
};
|
|
676
|
-
this.tailNames = Array.from({ length: n }, (_, i) => 'tail' + i);
|
|
677
|
-
this.ears = ['L', 'R'].map((S) => ({ name: 'ear' + S, base: J['earBase' + S], tip: J['earTip' + S] }));
|
|
678
|
-
// the pinna's own extremes (its skin tip and rim, bind, from the ear base), kept out of the ground
|
|
679
|
-
// with the tip joint: a dead bear's ear edge lay 13 mm in the ground with its tip joint clear
|
|
680
|
-
for (const E of this.ears) if (E.base && E.tip) Object.assign(E, skinMemo(data, E.name, () => earRim(data, E.name, E.base, E.tip)));
|
|
681
|
-
// (clearMulti's scratch: per point its clear range; the points and their floors)
|
|
682
|
-
this._cm = new Float64Array(2 * (EAR_PTS + 1));
|
|
683
|
-
this._cmV = Array.from({ length: EAR_PTS + 1 }, () => new THREE.Vector3());
|
|
684
|
-
this._cmF = new Float64Array(EAR_PTS + 1);
|
|
685
|
-
// ---- frames (preallocated, written by the body build)
|
|
686
|
-
const V = () => new THREE.Vector3(), Q = () => new THREE.Quaternion();
|
|
687
|
-
this.fr = {
|
|
688
|
-
qB: Q(), qS3: Q(), qCh: Q(), qS2: Q(), qS1: Q(), qPel: Q(), qHeading: Q(),
|
|
689
|
-
origin: V(), root: V(), chestMid: V(), neckBase: V(), lumbarMid: V(), lumbo: V(), tailBase: V(), hipC: V(), shC: V(),
|
|
690
|
-
F: V(), Lf: V(), pos: V(), comp: 0, bs: 1,
|
|
691
|
-
};
|
|
692
|
-
this.qBprev = new THREE.Quaternion();
|
|
693
|
-
this._headInit = false;
|
|
694
|
-
this._hcv = new Float64Array(10);
|
|
695
|
-
this._hc = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0]; // ground-clearance corrections (nose, chin, ear bases, ear tips, head sides, nose / chin again), rad
|
|
696
|
-
this._carry = [0, 0, 0];
|
|
697
|
-
this._stab = [0];
|
|
698
|
-
this._e = new THREE.Euler();
|
|
699
|
-
this._scale = new THREE.Vector3();
|
|
700
|
-
this._q = new THREE.Quaternion();
|
|
701
|
-
// ---- species extra bones (not driven by the engine: e.g. a twitching snout) follow their parent
|
|
702
|
-
// rigidly (bind-relative) every frame; species.motion.hooks.pose(engine, dt) may then move them.
|
|
703
|
-
this.hooks = species.motion?.hooks || {};
|
|
704
|
-
this.params = data.params || {};
|
|
705
|
-
const known = /^(pelvis|spine[123]|chest|neck[12]|head|jaw|ear[LR]|scapula[LR]|humerus[LR]|radius[LR]|metacarpus[LR]|fpaw[LR]|fhoof[LR]|femur[LR]|tibia[LR]|metatarsus[LR]|hpaw[LR]|hhoof[LR]|tail\d+)$/;
|
|
706
|
-
this.extraBones = [];
|
|
707
|
-
for (let i = 0; i < data.bones.length; i++) {
|
|
708
|
-
const b = data.bones[i];
|
|
709
|
-
if (known.test(b.name)) continue;
|
|
710
|
-
const pi = data.bones.findIndex((x) => x.name === b.parent);
|
|
711
|
-
if (pi < 0) continue;
|
|
712
|
-
const inv = skeleton.inverses ? skeleton.inverses[pi] : skeleton.bind[pi].clone().invert();
|
|
713
|
-
this.extraBones.push({ bone: skeleton.bones[i], parent: skeleton.bones[pi], rel: inv.clone().multiply(skeleton.bind[i]) });
|
|
714
|
-
}
|
|
715
|
-
// ---- actions / posture parameters
|
|
716
|
-
this.P = createPostureParams();
|
|
717
|
-
this.actionsLayer = new ActionLayer(this);
|
|
718
|
-
this.actions = this.actionsLayer.names;
|
|
719
|
-
// ---- attachments (bind-relative frames: +Z forward, +Y up at bind)
|
|
720
|
-
this.attachmentPoints = this._attachments(data);
|
|
721
|
-
this.reset(position || new THREE.Vector3(), heading);
|
|
722
|
-
}
|
|
723
|
-
|
|
724
|
-
_attachments(data) {
|
|
725
|
-
const J = this.J, cfg = this.cfg;
|
|
726
|
-
const at = (bone, p) => {
|
|
727
|
-
const i = data.bones.findIndex((b) => b.name === bone);
|
|
728
|
-
if (i < 0) return null;
|
|
729
|
-
const inv = this.sk.inverses ? this.sk.inverses[i] : this.sk.bind[i].clone().invert();
|
|
730
|
-
return { bone, local: new THREE.Matrix4().makeTranslation(p.x, p.y, p.z).premultiply(inv) };
|
|
731
|
-
};
|
|
732
|
-
const mouth = J.occiput.clone().add(cfg.mouthLocal);
|
|
733
|
-
let headP = J.occiput.clone().lerp(J.nose, 0.3);
|
|
734
|
-
if (data.eyes?.length) { headP = new THREE.Vector3(); for (const e of data.eyes) headP.add(new THREE.Vector3(...e.c)); headP.multiplyScalar(1 / data.eyes.length); }
|
|
735
|
-
const back = J.thoraxRear.clone().lerp(J.chestMid, 0.35);
|
|
736
|
-
back.y += cfg.back;
|
|
737
|
-
const out = { mouth: at('head', mouth), head: at('head', headP), back: at('spine3', back) };
|
|
738
|
-
// species attachment points (saddle, stirrups, reins ...): { name: { bone, from, to?, t?, offset? } }
|
|
739
|
-
// at lerp(joint from, joint to, t) + offset (m at the reference size, x = left), riding on `bone`
|
|
740
|
-
for (const [k, a] of Object.entries(this.species.motion?.attachments || {})) {
|
|
741
|
-
if (!J[a.from]) continue;
|
|
742
|
-
const p = J[a.from].clone();
|
|
743
|
-
if (a.to && J[a.to]) p.lerp(J[a.to], a.t ?? 0.5);
|
|
744
|
-
if (a.offset) p.add(new THREE.Vector3(a.offset[0], a.offset[1], a.offset[2]).multiplyScalar(cfg.s));
|
|
745
|
-
out[k] = at(a.bone, p);
|
|
746
|
-
}
|
|
747
|
-
for (const k of Object.keys(out)) if (!out[k]) delete out[k];
|
|
748
|
-
return out;
|
|
749
|
-
}
|
|
750
|
-
|
|
751
|
-
// ----------------------------------------------------------------- frames
|
|
752
|
-
forward(h, out) { return out.set(Math.sin(h), 0, Math.cos(h)); }
|
|
753
|
-
left(h, out) { return out.set(Math.cos(h), 0, -Math.sin(h)); }
|
|
754
|
-
// body-local (x = left, y = up, z = forward) -> world, about a ground point p with heading h
|
|
755
|
-
bodyToWorld(lx, ly, lz, h, p, out) {
|
|
756
|
-
const sh = Math.sin(h), ch = Math.cos(h);
|
|
757
|
-
return out.set(p.x + ch * lx + sh * lz, p.y + ly, p.z - sh * lx + ch * lz);
|
|
758
|
-
}
|
|
759
|
-
terrainH(x, z) {
|
|
760
|
-
const h = this.ground(x, z);
|
|
761
|
-
return Number.isFinite(h) ? h : 0;
|
|
762
|
-
}
|
|
763
|
-
terrainN(x, z, out) {
|
|
764
|
-
const e = 0.2 * this.cfg.k, h0 = this.terrainH(x, z);
|
|
765
|
-
return out.set(h0 - this.terrainH(x + e, z), e, h0 - this.terrainH(x, z + e)).normalize();
|
|
766
|
-
}
|
|
767
|
-
|
|
768
|
-
reset(p, heading = this.heading) {
|
|
769
|
-
const cfg = this.cfg;
|
|
770
|
-
this.pos.set(p.x, 0, p.z);
|
|
771
|
-
this.pos.y = this.terrainH(p.x, p.z);
|
|
772
|
-
this.heading = heading;
|
|
773
|
-
this.input.heading = heading;
|
|
774
|
-
this.speed = 0; this.yawRate = 0; this.yawAcc = 0;
|
|
775
|
-
this.velocity.set(0, 0, 0); this.lastVel.set(0, 0, 0);
|
|
776
|
-
this.gaitAt(0);
|
|
777
|
-
for (const leg of this.legs) {
|
|
778
|
-
this.neutralContact(leg.def, 0, 0, leg.plant, leg);
|
|
779
|
-
leg.contact.copy(leg.plant);
|
|
780
|
-
leg.state = 'stance';
|
|
781
|
-
leg.fwdInit = false;
|
|
782
|
-
this.terrainN(leg.plant.x, leg.plant.z, leg.plantN);
|
|
783
|
-
leg.protInit = false;
|
|
784
|
-
}
|
|
785
|
-
const base = this.pos.y;
|
|
786
|
-
this.height.reset(base); this.hipY.reset(base); this.shY.reset(base);
|
|
787
|
-
this.sag.on = false; this.sag.F.reset(0); this.sag.H.reset(0);
|
|
788
|
-
this.pitch.reset(Math.atan2(this.terrainH(p.x + Math.sin(heading) * cfg.zS, p.z + Math.cos(heading) * cfg.zS) - this.terrainH(p.x + Math.sin(heading) * cfg.zH, p.z + Math.cos(heading) * cfg.zH), cfg.zS - cfg.zH));
|
|
789
|
-
this._headInit = false;
|
|
790
|
-
this.tail.init = false;
|
|
791
|
-
this.qBprev.setFromAxisAngle(UP, heading);
|
|
792
|
-
}
|
|
793
|
-
|
|
794
|
-
// ----------------------------------------------------------------- gait
|
|
795
|
-
gaitAt(vn) {
|
|
796
|
-
const G = this.cfg.gaits, g = this.gait;
|
|
797
|
-
let i = 0;
|
|
798
|
-
while (i < G.length - 2 && vn > G[i + 1].v) i++;
|
|
799
|
-
const a = G[i], b = G[Math.min(i + 1, G.length - 1)];
|
|
800
|
-
const t = a === b ? 0 : smooth(0, 1, (vn - a.v) / (b.v - a.v));
|
|
801
|
-
g.name = t < 0.5 ? a.name : b.name;
|
|
802
|
-
g.blend = t;
|
|
803
|
-
// explicit stores (keyed stores of doubles would allocate)
|
|
804
|
-
g.f = lerp(a.f, b.f, t); g.D = lerp(a.D, b.D, t);
|
|
805
|
-
g.liftF = lerp(a.liftF, b.liftF, t); g.liftH = lerp(a.liftH, b.liftH, t);
|
|
806
|
-
g.bobF = lerp(a.bobF, b.bobF, t); g.bobH = lerp(a.bobH, b.bobH, t);
|
|
807
|
-
g.flex = lerp(a.flex, b.flex, t); g.lat = lerp(a.lat, b.lat, t); g.fold = lerp(a.fold, b.fold, t);
|
|
808
|
-
g.lead = lerp(a.lead, b.lead, t); g.heel = lerp(a.heel, b.heel, t); g.tail = lerp(a.tail, b.tail, t); g.drop = lerp(a.drop, b.drop, t);
|
|
809
|
-
g.width = lerp(a.width, b.width, t);
|
|
810
|
-
for (let j = 0; j < 4; j++) g.off[j] = wrap01(a.off[j] + d01(a.off[j], b.off[j]) * t);
|
|
811
|
-
if (vn > b.v) g.f = b.f + (vn - b.v) * 0.02;
|
|
812
|
-
const cfg = this.cfg;
|
|
813
|
-
g.f /= cfg.sq;
|
|
814
|
-
const s = cfg.s;
|
|
815
|
-
g.liftF *= s; g.liftH *= s; g.bobF *= s; g.bobH *= s; g.drop *= s; g.lead *= s;
|
|
816
|
-
g.gallop = smooth(cfg.gallopBlend[0], cfg.gallopBlend[1], vn);
|
|
817
|
-
// moving crouch: a moving quadruped carries its girdles a little lower than it stands, so its legs
|
|
818
|
-
// work from flexed joints and can sweep a full stance without reaching straight
|
|
819
|
-
// (no two strides alike: a few per cent of timing, foot lift and bounce, eased across the stride)
|
|
820
|
-
const sv = this.sv, sw = smooth(0, 1, this.phase), sg = 1 - 0.5 * smooth(cfg.gallopBlend[0], cfg.gallopBlend[1], vn);
|
|
821
|
-
for (let i = 0; i < 4; i++) sv.x[i] = lerp(sv.a[i], sv.b[i], sw) * sg * cfg.stride.vary;
|
|
822
|
-
g.f *= 1 + 0.025 * sv.x[0]; g.liftF *= 1 + 0.1 * sv.x[1]; g.liftH *= 1 + 0.1 * sv.x[2];
|
|
823
|
-
g.bobF *= 1 + 0.12 * sv.x[3]; g.bobH *= 1 + 0.12 * sv.x[3];
|
|
824
|
-
const cr = cfg.moveCrouch;
|
|
825
|
-
g.drop += cr * Math.min(cfg.yS, cfg.yH) * smooth(0.02, 0.5, vn) * lerp(1, lerp(0.6, cfg.legs[0].foot.type === 'unguligrade' ? 1.5 : 0.8, cfg.stride.gallop), g.gallop);
|
|
826
|
-
// per-leg duty factor: a stance may not sweep further than the leg reaches. The table's duty
|
|
827
|
-
// factor asks for a stance sweep of v D / f; where that exceeds the leg's reachable sweep (from
|
|
828
|
-
// its girdle's current height, see reachSpan) the stance is shortened (longer swing / flight),
|
|
829
|
-
// so a planted foot is never dragged and the stance profiles (heel lift, flex) play out in full.
|
|
830
|
-
const v = this.speed;
|
|
831
|
-
for (let j = 0; j < 4; j++) {
|
|
832
|
-
const sw = this.legs && this.cfg.reachDuty ? this.legs[j].sweep * this.cfg.reachDuty * (j < 2 ? 1 + (this.cfg.legs[0].foot.type === 'unguligrade' ? 0.45 : 0) * g.gallop * this.cfg.stride.gallop : 1) : 0;
|
|
833
|
-
g.DL[j] = v > 1e-3 && sw > 0 ? clamp(g.f * sw / v, 0.04, g.D) : g.D;
|
|
834
|
-
}
|
|
835
|
-
return g;
|
|
836
|
-
}
|
|
837
|
-
|
|
838
|
-
// sideways part of the landing lead while crabbing: side-steps, only as far as the leg abducts
|
|
839
|
-
crabX(L, lead) {
|
|
840
|
-
if (!this.crab) return 0;
|
|
841
|
-
const m = 0.12 * L.len;
|
|
842
|
-
return clamp(lead * Math.sin(this.crab), -m, m);
|
|
843
|
-
}
|
|
844
|
-
|
|
845
|
-
// body-local z of the point the body turns about, the point of its axis that moves along the
|
|
846
|
-
// heading (see step): by the turn's radius R = v / yaw rate against the girdle span d. In a turn
|
|
847
|
-
// tighter than the body (R < 0.8 d: standing, a sharp turn at the walk) the hip joints: the
|
|
848
|
-
// forequarters come round the hindquarters and the hind feet step on their line, a quadruped's turn
|
|
849
|
-
// on the haunches; in a wide turn (R > 2.5 d) the girdles' midpoint, where the forefeet's and hind
|
|
850
|
-
// feet's tracks are one circle. (0 in follow mode: the followed body places the root)
|
|
851
|
-
turnCentre() {
|
|
852
|
-
if (this.input.follow) return 0;
|
|
853
|
-
const cfg = this.cfg, R = this.speed / Math.max(Math.abs(this.yawRate), 1e-4);
|
|
854
|
-
return lerp(cfg.zH, 0.5 * (cfg.zS + cfg.zH), smooth(0.8, 2.5, R / (cfg.zS - cfg.zH)));
|
|
855
|
-
}
|
|
856
|
-
|
|
857
|
-
// A girdle under the locomotion's lateral spine bend (last pose): where its centre sits relative to
|
|
858
|
-
// where the unbent body would carry it (heading frame: x left, z forward) and its yaw (rad), from the
|
|
859
|
-
// bend of each spine joint (see buildBody). out = [dx, dz, yaw]
|
|
860
|
-
girdleBend(front, out) {
|
|
861
|
-
const c = this.cfg.chain;
|
|
862
|
-
if (front) {
|
|
863
|
-
const a1 = 0.2 * this._latF, a2 = 0.4 * this._latF;
|
|
864
|
-
const c1 = Math.cos(a1), s1 = Math.sin(a1), c2 = Math.cos(a2), s2 = Math.sin(a2);
|
|
865
|
-
out[0] = c[0] * c1 + c[1] * s1 + c[2] * c2 + c[3] * s2 - c[0] - c[2];
|
|
866
|
-
out[1] = -c[0] * s1 + c[1] * c1 - c[2] * s2 + c[3] * c2 - c[1] - c[3];
|
|
867
|
-
out[2] = a2;
|
|
868
|
-
} else {
|
|
869
|
-
const b1 = -0.15 * this._latH, b2 = -0.35 * this._latH, b3 = -0.6 * this._latH;
|
|
870
|
-
const c1 = Math.cos(b1), s1 = Math.sin(b1), c2 = Math.cos(b2), s2 = Math.sin(b2), c3 = Math.cos(b3), s3 = Math.sin(b3);
|
|
871
|
-
out[0] = c[4] * c1 + c[5] * s1 + c[6] * c2 + c[7] * s2 + c[8] * c3 + c[9] * s3 - c[4] - c[6] - c[8];
|
|
872
|
-
out[1] = -c[4] * s1 + c[5] * c1 - c[6] * s2 + c[7] * c2 - c[8] * s3 + c[9] * c3 - c[5] - c[7] - c[9];
|
|
873
|
-
out[2] = b3;
|
|
874
|
-
}
|
|
875
|
-
return out;
|
|
876
|
-
}
|
|
877
|
-
|
|
878
|
-
// body frame turned by the locomotion's lateral bend at a leg's girdle (the chest's yaw for a
|
|
879
|
-
// foreleg, the pelvis's for a hind leg, see girdleBend): the frame its paw and its knee / elbow follow
|
|
880
|
-
girdleFrame(L) {
|
|
881
|
-
return tqc(this.fr.qB).multiply(tqa(UP, L.front ? 0.4 * this._latF : -0.6 * this._latH));
|
|
882
|
-
}
|
|
883
|
-
|
|
884
|
-
neutralContact(L, lead, tAhead, out, leg) {
|
|
885
|
-
const g = this.gait, st = this.cfg.stance;
|
|
886
|
-
const width = lerp(L.cx, L.cx * st.gallopWidth, g.gallop) * st.width * g.width + st.sprawl * L.len * 0.35;
|
|
887
|
-
const hp = this.heading + this.yawRate * tAhead;
|
|
888
|
-
// (the body travels along heading + crab: sideways while a followed body carries it so)
|
|
889
|
-
const hm = this.heading + this.crab + this.yawRate * tAhead * 0.5;
|
|
890
|
-
const d = this.speed * tAhead;
|
|
891
|
-
// (... and swings about its turn centre while it turns)
|
|
892
|
-
const zc = this.turnCentre();
|
|
893
|
-
const bx = this.pos.x + Math.sin(hm) * d + zc * (Math.sin(this.heading) - Math.sin(hp)), bz = this.pos.z + Math.cos(hm) * d + zc * (Math.cos(this.heading) - Math.cos(hp));
|
|
894
|
-
// (the lead, half a stance sweep ahead of the girdle, lies along the motion too)
|
|
895
|
-
let lx = width * L.s + this.crabX(L, lead), lz = L.cz + lead * Math.cos(this.crab);
|
|
896
|
-
if (leg) { lx += leg.idleOff.x; lz += leg.idleOff.z; }
|
|
897
|
-
// (under the girdle where the spine's lateral bend carries it, turned with it: a turn bends the
|
|
898
|
-
// chest into it and the pelvis back along the track, and a foot placed in the unbent frame landed
|
|
899
|
-
// up to 0.15 leg lengths to the side of its girdle, the leg slanted)
|
|
900
|
-
const gb = this.girdleBend(L.front, this._gb);
|
|
901
|
-
if (gb[2] !== 0) {
|
|
902
|
-
const rz = lz - L.gz, c = Math.cos(gb[2]), sn = Math.sin(gb[2]);
|
|
903
|
-
lz = L.gz + gb[1] - lx * sn + rz * c;
|
|
904
|
-
lx = gb[0] + lx * c + rz * sn;
|
|
905
|
-
}
|
|
906
|
-
this._fl[0] = lx; this._fl[1] = lz; // (body-local, for the swing's landLocal)
|
|
907
|
-
const sh = Math.sin(hp), ch = Math.cos(hp);
|
|
908
|
-
out.set(bx + ch * lx + sh * lz, 0, bz - sh * lx + ch * lz);
|
|
909
|
-
out.y = this.terrainH(out.x, out.z);
|
|
910
|
-
return out;
|
|
911
|
-
}
|
|
912
|
-
|
|
913
|
-
worldToBody(w, out) {
|
|
914
|
-
const dx = w.x - this.pos.x, dz = w.z - this.pos.z;
|
|
915
|
-
const sh = Math.sin(this.heading), ch = Math.cos(this.heading);
|
|
916
|
-
return out.set(dx * ch - dz * sh, w.y, dx * sh + dz * ch);
|
|
917
|
-
}
|
|
918
|
-
|
|
919
|
-
// ----------------------------------------------------------------- public API
|
|
920
|
-
play(name, opts) { return this.actionsLayer.play(name, opts || {}); }
|
|
921
|
-
stop(name) { this.actionsLayer.stop(name); }
|
|
922
|
-
setLod(level) { this.lod = clamp(level | 0, 0, 2); return this; }
|
|
923
|
-
dispose() { this.actionsLayer.clear(); }
|
|
924
|
-
|
|
925
|
-
update(dt) {
|
|
926
|
-
// a zero / invalid step (first animation frame, paused tab) would turn finite differences into NaN
|
|
927
|
-
if (!(dt > 1e-4) || !Number.isFinite(dt)) dt = 1e-4;
|
|
928
|
-
dt = Math.min(dt, 1 / 20);
|
|
929
|
-
this.frame++;
|
|
930
|
-
this.frameDt = dt;
|
|
931
|
-
this.drive(dt);
|
|
932
|
-
this.actionsLayer.update(dt);
|
|
933
|
-
const sub = dt > 1 / 70 && this.lod === 0 ? 2 : 1;
|
|
934
|
-
for (let i = 0; i < sub; i++) this.step(dt / sub);
|
|
935
|
-
this.pose(dt);
|
|
936
|
-
this.publish();
|
|
937
|
-
}
|
|
938
|
-
|
|
939
|
-
// steering input -> desired speed / heading (moveTo, follow, turn-on-the-spot for look targets)
|
|
940
|
-
drive(dt) {
|
|
941
|
-
const inp = this.input, cfg = this.cfg;
|
|
942
|
-
this.wantSpeed = clamp(inp.speed || 0, 0, cfg.maxSpeed);
|
|
943
|
-
this.wantHeading = inp.heading ?? this.heading;
|
|
944
|
-
if (inp.target && !inp.follow) {
|
|
945
|
-
const dx = inp.target.x - this.pos.x, dz = inp.target.z - this.pos.z;
|
|
946
|
-
const d = Math.hypot(dx, dz);
|
|
947
|
-
const stopR = 0.35 * cfg.k + 0.02 * this.speed;
|
|
948
|
-
if (d < stopR) { inp.target = null; inp.speed = 0; this.wantSpeed = 0; this.emit('arrive', {}); }
|
|
949
|
-
else {
|
|
950
|
-
const want = Math.atan2(dx, dz);
|
|
951
|
-
inp.heading = want;
|
|
952
|
-
this.wantHeading = want;
|
|
953
|
-
const ang = Math.abs(angDiff(this.heading, want));
|
|
954
|
-
const brake = Math.sqrt(Math.max(0, 2 * cfg.decel * 0.8 * (d - stopR)));
|
|
955
|
-
this.wantSpeed = Math.min(inp.targetSpeed || 3, cfg.maxSpeed, brake + 0.25, ang > 1.2 ? 1.5 * cfg.sk : 1e9);
|
|
956
|
-
}
|
|
957
|
-
}
|
|
958
|
-
// posture: sitting / lying / dead animals stand up before they move
|
|
959
|
-
if (this.actionsLayer.holdsStill()) {
|
|
960
|
-
if (this.wantSpeed > 0.05 && !inp.follow) this.actionsLayer.requestStand();
|
|
961
|
-
this.wantSpeed = 0;
|
|
962
|
-
this.wantHeading = this.heading;
|
|
963
|
-
} else if (!inp.follow && this.wantSpeed < 0.05) {
|
|
964
|
-
// turn on the spot toward a look target that is behind
|
|
965
|
-
const lt = inp.look || null;
|
|
966
|
-
if (lt) {
|
|
967
|
-
const want = Math.atan2(lt.x - this.pos.x, lt.z - this.pos.z);
|
|
968
|
-
const off = angDiff(this.heading, want);
|
|
969
|
-
if (Math.abs(off) > cfg.head.yaw * 1.05) this._turnTo = want;
|
|
970
|
-
else if (this._turnTo !== undefined && Math.abs(off) < 0.3) this._turnTo = undefined;
|
|
971
|
-
if (this._turnTo !== undefined) { this.wantHeading = this._turnTo; inp.heading = this._turnTo; }
|
|
972
|
-
} else this._turnTo = undefined;
|
|
973
|
-
}
|
|
974
|
-
}
|
|
975
|
-
|
|
976
|
-
// ----------------------------------------------------------------- simulation step
|
|
977
|
-
step(dt) {
|
|
978
|
-
this.time += dt;
|
|
979
|
-
const cfg = this.cfg, inp = this.input, P = this.P;
|
|
980
|
-
const F = this.forward(this.heading, tv());
|
|
981
|
-
if (inp.follow) {
|
|
982
|
-
// root placed from outside; speed and heading from its velocity
|
|
983
|
-
const fv = inp.follow.velocity;
|
|
984
|
-
const vx = fv ? fv.x : 0, vz = fv ? fv.z : 0;
|
|
985
|
-
const sp = Math.hypot(vx, vz);
|
|
986
|
-
this.speed = Math.max(0, this.speedS.step(sp, 10, dt));
|
|
987
|
-
let hT = inp.follow.heading;
|
|
988
|
-
if (hT === null || hT === undefined) hT = sp > 0.25 * cfg.sk ? Math.atan2(vx, vz) : this.heading;
|
|
989
|
-
const wMax = cfg.turnRate * 1.8;
|
|
990
|
-
// feed-forward the turn rate of the followed velocity (a pure P controller lags by
|
|
991
|
-
// yawRate / 6: ~11 deg on a 5 m circle, and the animal crabs sideways over its feet)
|
|
992
|
-
if (this._fH === undefined) {
|
|
993
|
-
this._fH = hT; this._fW = 0;
|
|
994
|
-
// follow starts while the followed body is already moving and the animal stands: the body is
|
|
995
|
-
// carried at that speed from this frame on, so the gait starts at it (the speed filter
|
|
996
|
-
// started from 0 and the feet stayed planted while the body moved away from them), and the
|
|
997
|
-
// animal takes the body's heading, as it takes its position (a standing animal carried
|
|
998
|
-
// sideways at 90 deg dragged its planted forefoot 2.5-4 % S before its heading caught up)
|
|
999
|
-
if (this.speed < 0.05 * cfg.sk && sp > 0.25 * cfg.sk && !this.air.active) {
|
|
1000
|
-
if (Math.abs(angDiff(this.heading, hT)) > 0.3) this.reset(tv(inp.follow.position.x, 0, inp.follow.position.z), hT);
|
|
1001
|
-
this.speedS.reset(sp); this.speed = sp;
|
|
1002
|
-
this._followStart = true;
|
|
1003
|
-
}
|
|
1004
|
-
}
|
|
1005
|
-
this._fW = expTo(this._fW, angDiff(this._fH, hT) / dt, dt, 0.12);
|
|
1006
|
-
this._fH = hT;
|
|
1007
|
-
const wT = clamp(this._fW + angDiff(this.heading, hT) * 6, -wMax, wMax);
|
|
1008
|
-
const dw = clamp(wT - this.yawRate, -20 * dt, 20 * dt);
|
|
1009
|
-
this.yawRate += dw; this.yawAcc = dw / dt;
|
|
1010
|
-
this.heading += this.yawRate * dt;
|
|
1011
|
-
const fp = inp.follow.position;
|
|
1012
|
-
this.pos.x = fp.x; this.pos.z = fp.z;
|
|
1013
|
-
this.forward(this.heading, F);
|
|
1014
|
-
this.velocity.set(vx, fv ? fv.y : 0, vz);
|
|
1015
|
-
this.crab = sp > 1e-4 ? angDiff(this.heading, Math.atan2(vx, vz)) * smooth(0.03, 0.25, sp / cfg.sq) : 0;
|
|
1016
|
-
} else {
|
|
1017
|
-
this.crab = 0;
|
|
1018
|
-
// speed & heading dynamics
|
|
1019
|
-
let vT = this.wantSpeed * P.speedScale;
|
|
1020
|
-
if (this.air.active) vT = this.speed; // no traction in the air
|
|
1021
|
-
const vn = this.speed / cfg.sq;
|
|
1022
|
-
const acc = vT > this.speed ? lerp(cfg.accel[0], cfg.accel[1], smooth(2, 10, vn)) : cfg.decel;
|
|
1023
|
-
this.speed += clamp(vT - this.speed, -acc * dt, acc * dt);
|
|
1024
|
-
if (this.speedOverride !== null) this.speed = this.speedOverride; // (jump push-off)
|
|
1025
|
-
if (this.air.active) this.speed = this.air.vh;
|
|
1026
|
-
const v = this.speed;
|
|
1027
|
-
// pivoting on the spot is slower than turning on the move (the feet have to keep up)
|
|
1028
|
-
const wMax = Math.min(cfg.turnRate * lerp(0.55, 1, smooth(0.3, 2, vn)), cfg.latAccelMax / Math.max(v, 0.5 * cfg.sk)) * (this.air.active ? 0 : 1);
|
|
1029
|
-
// heading: a cascade, heading error -> yaw rate (capped at wMax) -> yaw acceleration (capped) ->
|
|
1030
|
-
// the acceleration itself, which follows with a lag (and at most 40 / k^1.5 rad/s^3): a linear
|
|
1031
|
-
// third-order response with a triple pole at p = 4.5 / sqrt(k) rad/s (every constant dynamically
|
|
1032
|
-
// similar), no overshoot, the yaw acceleration continuous, and saturation (a long turn at the rate
|
|
1033
|
-
// limit) settles without ringing. It was a proportional yaw-rate target slewed at a fixed 8 rad/s^2:
|
|
1034
|
-
// the yaw acceleration jumped from 0 to 8 in one frame on every heading command (jerk 290-770
|
|
1035
|
-
// rad/s^3: the body snapped into a turn), and at the same rate for a rabbit as for a horse
|
|
1036
|
-
const pp = 4.5 / cfg.sk, aMax = 8 / cfg.k, jMax = 40 / (cfg.k * cfg.sk);
|
|
1037
|
-
const wd = clamp(angDiff(this.heading, this.wantHeading) * pp / 3, -wMax, wMax);
|
|
1038
|
-
const ad = clamp((wd - this.yawRate) * pp, -aMax, aMax);
|
|
1039
|
-
this.yawAcc += clamp((ad - this.yawAcc) * (1 - Math.exp(-3 * pp * dt)), -jMax * dt, jMax * dt);
|
|
1040
|
-
this.yawRate += this.yawAcc * dt;
|
|
1041
|
-
const h0 = this.heading;
|
|
1042
|
-
this.heading += this.yawRate * dt;
|
|
1043
|
-
this.forward(this.heading, F);
|
|
1044
|
-
this.velocity.copy(F).multiplyScalar(v);
|
|
1045
|
-
this.pos.addScaledVector(F, v * dt);
|
|
1046
|
-
// the body turns about a point of its axis (turnCentre): the hindquarters in a turn tighter than
|
|
1047
|
-
// the body (standing: the forelegs walk round them, the hind feet step in place), the girdles'
|
|
1048
|
-
// midpoint in a wide one. It turned about its middle whatever the turn: a standing pivot swung the
|
|
1049
|
-
// hindquarters and tail through a wide arc (a cheetah's hips travelled 1.5 m for a half turn) and
|
|
1050
|
-
// a sharp turn at the walk swung them out to the outside of the turn first, like a pendulum
|
|
1051
|
-
const zc = this.turnCentre();
|
|
1052
|
-
if (zc !== 0) {
|
|
1053
|
-
const dx = zc * (Math.sin(h0) - F.x), dz = zc * (Math.cos(h0) - F.z);
|
|
1054
|
-
this.pos.x += dx; this.pos.z += dz;
|
|
1055
|
-
this.velocity.x += dx / dt; this.velocity.z += dz / dt;
|
|
1056
|
-
}
|
|
1057
|
-
// shove (hit stagger)
|
|
1058
|
-
if (this.push.lengthSq() > 1e-8) {
|
|
1059
|
-
this.pos.addScaledVector(this.push, dt);
|
|
1060
|
-
this.velocity.add(this.push);
|
|
1061
|
-
this.push.multiplyScalar(Math.exp(-dt * 6));
|
|
1062
|
-
}
|
|
1063
|
-
}
|
|
1064
|
-
if (this.heading > Math.PI * 64 || this.heading < -Math.PI * 64) this.heading = angDiff(0, this.heading);
|
|
1065
|
-
this.pos.y = this.terrainH(this.pos.x, this.pos.z);
|
|
1066
|
-
this.accel.copy(this.velocity).sub(this.lastVel).divideScalar(dt);
|
|
1067
|
-
this.lastVel.copy(this.velocity);
|
|
1068
|
-
const v = this.speed, vn = v / cfg.sq;
|
|
1069
|
-
// exertion
|
|
1070
|
-
const hs = cfg.breath.heatSpeed;
|
|
1071
|
-
this.heat = clamp(this.heat + dt * (vn > hs ? (vn - hs) * 0.012 : -0.035), 0, 1);
|
|
1072
|
-
|
|
1073
|
-
// jump flight
|
|
1074
|
-
const air = this.air;
|
|
1075
|
-
if (air.active) {
|
|
1076
|
-
air.t += dt;
|
|
1077
|
-
const yBody = air.y0 + air.vy * air.t - 4.905 * air.t * air.t;
|
|
1078
|
-
this.lift = yBody - this.pos.y;
|
|
1079
|
-
if (air.t > 0.05 && this.lift <= 0 && air.vy - 9.81 * air.t < 0) this.touchDown();
|
|
1080
|
-
}
|
|
1081
|
-
|
|
1082
|
-
// gait
|
|
1083
|
-
if (this.frame > 2) for (const leg of this.legs) this.reachSpan(leg);
|
|
1084
|
-
const vEff = Math.max(vn, Math.abs(this.yawRate) * 0.8);
|
|
1085
|
-
const moving = vEff > 0.06;
|
|
1086
|
-
// input.gait forces a gait: its pattern (stride frequency, duty, phase offsets, body style) at the
|
|
1087
|
-
// nearest speed of its band, whatever the actual speed (the stride length makes up the difference)
|
|
1088
|
-
// (eased in and out with a 0.12 s time constant: switching the band off in the frame the animal
|
|
1089
|
-
// came to a halt snapped the gait parameters - heel lift, carpal fold - of a paw still in its
|
|
1090
|
-
// swing from the forced gait's to standing, a 200 rad/s* pop of the metacarpus)
|
|
1091
|
-
const band = inp.gait ? cfg.gaitBands[inp.gait] : null;
|
|
1092
|
-
this._bandW = expTo(this._bandW || 0, band && moving ? 1 : 0, dt, 0.12);
|
|
1093
|
-
if (band) this._band = band;
|
|
1094
|
-
const bnd = this._band;
|
|
1095
|
-
const g = this.gaitAt(bnd && this._bandW > 1e-4 ? lerp(vEff, clamp(vEff, bnd[0], bnd[1]), this._bandW) : vEff);
|
|
1096
|
-
if (moving) {
|
|
1097
|
-
const ph0 = this.phase;
|
|
1098
|
-
this.phase = wrap01(this.phase + g.f * dt);
|
|
1099
|
-
if (this.phase < ph0) { const sv = this.sv; for (let i = 0; i < 4; i++) { sv.a[i] = sv.b[i]; sv.b[i] = this.randS() * 2 - 1; } }
|
|
1100
|
-
}
|
|
1101
|
-
const st = this.state.stats;
|
|
1102
|
-
st.gait = moving ? g.name : 'stand';
|
|
1103
|
-
st.freq = moving ? g.f : 0;
|
|
1104
|
-
st.duty = g.D;
|
|
1105
|
-
|
|
1106
|
-
const legGait = P.legGait; // < 1 while a posture owns the legs
|
|
1107
|
-
if (this._followStart) {
|
|
1108
|
-
// (follow start at speed: the legs join the gait where its phase is - a leg past its stance
|
|
1109
|
-
// window swings now, instead of every foot holding a standing plant for the first frames)
|
|
1110
|
-
this._followStart = false;
|
|
1111
|
-
if (moving && legGait > 0.5) {
|
|
1112
|
-
for (const leg of this.legs) {
|
|
1113
|
-
const D = g.DL[leg.def.idx], p = wrap01(this.phase - g.off[leg.def.idx]);
|
|
1114
|
-
if (leg.state !== 'stance' || p < D) continue;
|
|
1115
|
-
leg.windowUsed = true;
|
|
1116
|
-
const remain = Math.max((1 - p) / g.f, 0.4 * (1 - D) / g.f);
|
|
1117
|
-
this.startSwing(leg, remain, g, false);
|
|
1118
|
-
leg.track = (1 - p) / g.f >= 0.4 * (1 - D) / g.f;
|
|
1119
|
-
}
|
|
1120
|
-
}
|
|
1121
|
-
}
|
|
1122
|
-
let swinging = 0;
|
|
1123
|
-
for (const leg of this.legs) if (leg.state === 'swing') swinging++;
|
|
1124
|
-
for (const leg of this.legs) {
|
|
1125
|
-
const L = leg.def;
|
|
1126
|
-
const p = wrap01(this.phase - g.off[L.idx]);
|
|
1127
|
-
const D = g.DL[L.idx];
|
|
1128
|
-
if (p < leg.prevPhase - 0.5) leg.windowUsed = false;
|
|
1129
|
-
if (air.active) {
|
|
1130
|
-
// in the air (feet tucked by the action layer): planted feet push off into a swing from
|
|
1131
|
-
// where they are, and a swing that ends in the air simply starts over (no footfall)
|
|
1132
|
-
if (leg.state === 'stance') { leg.windowUsed = true; this.startSwing(leg, Math.max(0.2, air.T * 0.6), g, false); }
|
|
1133
|
-
leg.swingT += dt / leg.swingDur;
|
|
1134
|
-
leg.w = clamp(leg.swingT, 0, 1);
|
|
1135
|
-
const remain = (1 - leg.w) * leg.swingDur;
|
|
1136
|
-
// the landing spot eases to where the stride will want the foot (the same lead as a ground
|
|
1137
|
-
// swing; a swing that was under way at take-off keeps its path: its target jumping would
|
|
1138
|
-
// snap the paw)
|
|
1139
|
-
const leadA = moving ? Math.min(v * (g.D / g.f) * 0.5 + (L.gz - L.cz) * smooth(0.05, 0.6, vn), L.leadMax) + g.lead : 0;
|
|
1140
|
-
const ea = 1 - Math.exp(-dt * 5);
|
|
1141
|
-
leg.landLocal.x += (L.cx * L.s + leg.idleOff.x - leg.landLocal.x) * ea;
|
|
1142
|
-
leg.landLocal.z += (L.cz + leadA + leg.idleOff.z - leg.landLocal.z) * ea;
|
|
1143
|
-
leg.landLocal.y = 0;
|
|
1144
|
-
this.neutralContact(L, leg.landLocal.z - L.cz - leg.idleOff.z, remain, leg.land, leg);
|
|
1145
|
-
if (leg.swingT >= 1) { leg.plant.copy(leg.land); leg.plantN.set(0, 1, 0); leg.betaLast = 0; this.startSwing(leg, Math.max(0.2, air.T * 0.6), g, false); }
|
|
1146
|
-
} else if (leg.state === 'stance') {
|
|
1147
|
-
leg.stanceTime += dt;
|
|
1148
|
-
// stance progress u: 0 at touchdown -> 1 at the expected lift-off. Integrated (continuous and
|
|
1149
|
-
// monotone) rather than read from the phase: a swing timed at another stride frequency or
|
|
1150
|
-
// gait (speeding up, braking, gait changes) lands off its phase offset, and u = p / D would
|
|
1151
|
-
// then start near 1 (a full heel lift in the first stance frame). In a steady gait this
|
|
1152
|
-
// equals p / D. ps: signed phase (a foot that landed just before its offset is early, not late)
|
|
1153
|
-
const ps = p > (1 + D) * 0.5 ? p - 1 : p;
|
|
1154
|
-
// (a stance lasts at least 0.3 D / f, see the lift-off window below)
|
|
1155
|
-
const tRem = Math.max((D - ps) / g.f, 0.3 * D / g.f - leg.stanceTime);
|
|
1156
|
-
// (at most twice the pace planned at touchdown: when the duty factor or the gait changes
|
|
1157
|
-
// under a planted foot, the heel must not snap up)
|
|
1158
|
-
if (leg.stanceTime <= dt) leg.uPace = g.f / D;
|
|
1159
|
-
leg.u = Math.min(1, leg.u + Math.min(tRem > dt ? (1 - leg.u) * (dt / tRem) : 1, 2 * dt * leg.uPace));
|
|
1160
|
-
const sc = cfg.stance.scuff;
|
|
1161
|
-
const slide = sc[2] * smooth(sc[0], sc[1], vn);
|
|
1162
|
-
if (slide > 0 && moving) {
|
|
1163
|
-
leg.plant.addScaledVector(F, v * slide * dt);
|
|
1164
|
-
leg.plant.y = this.terrainH(leg.plant.x, leg.plant.z);
|
|
1165
|
-
}
|
|
1166
|
-
if (moving) {
|
|
1167
|
-
// early lift-off once the planted foot trails too far behind its girdle
|
|
1168
|
-
const jw = this.bodyToWorld(L.gx, 0, L.gz, this.heading, this.pos, tv());
|
|
1169
|
-
const behind = (leg.plant.x - jw.x) * F.x + (leg.plant.z - jw.z) * F.z;
|
|
1170
|
-
// ... or once the leg can no longer reach it even with the heel fully raised
|
|
1171
|
-
// (predicted to the coming pose from the rate of the last two solves: at a gallop the paw
|
|
1172
|
-
// falls ~20 cm behind per frame, so waiting for the overreach itself drags it for a frame)
|
|
1173
|
-
// (over a fixed horizon of a 60 Hz frame x sqrt(k), not the frame: at 240 Hz one frame looked
|
|
1174
|
-
// 4 ms ahead, and a stance leg pulled toward full extension after a landing reached the IK's
|
|
1175
|
-
// singular straight-leg region before it lifted: an elbow snap the 60 Hz run never showed)
|
|
1176
|
-
const ovNext = leg.overreach + Math.max(0, leg.ovRate) * Math.max(this.frameDt, cfg.sk / 60);
|
|
1177
|
-
// ... or once it is behind the leg's reach with the heel raised (this frame's pose could not
|
|
1178
|
-
// reach it: at a sprint the paw falls 45 cm behind per frame)
|
|
1179
|
-
const rel = (leg.plant.x - this.pos.x) * F.x + (leg.plant.z - this.pos.z) * F.z - L.cz;
|
|
1180
|
-
// (with the heel the paw has now: reachBL, see reachSpan)
|
|
1181
|
-
const gone = this.frame > 2 && rel < leg.reachBL && behind < 0;
|
|
1182
|
-
// ... or once the leg's last IK solve, its girdle joint carried by the body's motion since
|
|
1183
|
-
// then, no longer reaches the planted foot's IK targets (the reach above is a model: a
|
|
1184
|
-
// rabbit's forefoot planted under its shoulder at a bound in a banked turn needed 103 % of
|
|
1185
|
-
// the leg one frame later while the model still reached it). (Over 1.02: a planted paw rolls
|
|
1186
|
-
// up about its toe to meet the last per cent, as a sprinting cheetah's do every stride)
|
|
1187
|
-
let ovP = 0;
|
|
1188
|
-
if (leg.ikM1 > 0) {
|
|
1189
|
-
const dh = this.heading - this._poseH, c = Math.cos(dh), sn = Math.sin(dh), fp = this.fr.pos;
|
|
1190
|
-
const jx = leg.ikJ.x - fp.x, jz = leg.ikJ.z - fp.z;
|
|
1191
|
-
const px = this.pos.x + jx * c + jz * sn, pz = this.pos.z - jx * sn + jz * c, py = leg.ikJ.y;
|
|
1192
|
-
ovP = Math.hypot(leg.ikT1.x - px, leg.ikT1.y - py, leg.ikT1.z - pz) / leg.ikM1;
|
|
1193
|
-
if (leg.ikM2 > 0) ovP = Math.max(ovP, Math.hypot(leg.ikT2.x - px, leg.ikT2.y - py, leg.ikT2.z - pz) / leg.ikM2);
|
|
1194
|
-
}
|
|
1195
|
-
// (a foot the leg cannot reach leaves the ground after two frames of stance at the latest,
|
|
1196
|
-
// wherever the phase is: dragging it is worse than an early step; but not before the frame's
|
|
1197
|
-
// pose has shown it planted: a sprinting cat's forefoot, planted and lifted within one sub
|
|
1198
|
-
// step, never touched the ground in any frame)
|
|
1199
|
-
const over = (behind < -L.behindMax || gone || (ovNext > 1.0 && (behind < 0 || Math.abs(this.yawRate) > 0.5)) || (ovP > 1.02 && behind < 0)) && leg.stanceTime > Math.max(Math.min(0.25 * D / g.f, 1.5 * this.frameDt), 0.99 * this.frameDt);
|
|
1200
|
-
if (legGait > 0.5) {
|
|
1201
|
-
leg.mode = 'gait';
|
|
1202
|
-
if (((ps >= D && leg.stanceTime > 0.3 * D / g.f) || over) && !leg.windowUsed) {
|
|
1203
|
-
leg.windowUsed = true;
|
|
1204
|
-
// never skip a swing (a late window would drag the foot through a whole cycle):
|
|
1205
|
-
// a short remainder gets a minimum swing that runs into the next cycle
|
|
1206
|
-
const remain = Math.max((1 - p) / g.f, 0.4 * (1 - D) / g.f);
|
|
1207
|
-
this.startSwing(leg, remain, g, false);
|
|
1208
|
-
// a normal swing lands on its phase offset even if the stride frequency changes meanwhile
|
|
1209
|
-
// (speeding up / braking); a forced minimum swing runs on its own clock
|
|
1210
|
-
leg.track = (1 - p) / g.f >= 0.4 * (1 - D) / g.f;
|
|
1211
|
-
}
|
|
1212
|
-
} else if (over && P.holdFeet < 0.5) {
|
|
1213
|
-
// a posture or action holds the legs out of the gait (legGait <= 0.5: the push-off and
|
|
1214
|
-
// landing of a jump at speed, a kick, dying on the move) while the body still moves: a
|
|
1215
|
-
// planted foot it leaves behind catches up with a minimum step instead of being dragged
|
|
1216
|
-
// (the bear's forefoot trailed 45 cm behind its wrist through a running jump's push-off,
|
|
1217
|
-
// heel raised 74 deg, and snapped forward at take-off: a 104 rad/s* elbow)
|
|
1218
|
-
leg.windowUsed = true;
|
|
1219
|
-
leg.mode = 'gait';
|
|
1220
|
-
this.startSwing(leg, 0.4 * (1 - D) / g.f, g, false);
|
|
1221
|
-
leg.track = false;
|
|
1222
|
-
}
|
|
1223
|
-
} else if (legGait > 0.5) {
|
|
1224
|
-
// standing: step to neutral if displaced
|
|
1225
|
-
leg.u = lerp(leg.u, 0.3, 1 - Math.exp(-dt * 6));
|
|
1226
|
-
const nC = this.neutralContact(L, 0, 0, tv(), leg);
|
|
1227
|
-
const err = nC.distanceTo(leg.plant);
|
|
1228
|
-
// (or when the planted paw has been left twisted by more than ~35 deg by a turn on the spot)
|
|
1229
|
-
const twist = leg.fwdInit ? leg.plantFwd.x * F.x + leg.plantFwd.z * F.z : 1;
|
|
1230
|
-
if ((err > 0.085 * cfg.k || twist < 0.82) && swinging === 0 && leg.stanceTime > 0.25) {
|
|
1231
|
-
this.startSwing(leg, 0.32 * cfg.sk, g, true);
|
|
1232
|
-
swinging++;
|
|
1233
|
-
}
|
|
1234
|
-
}
|
|
1235
|
-
} else {
|
|
1236
|
-
if (leg.track && moving && leg.mode === 'gait') {
|
|
1237
|
-
// w runs to 1 when the phase reaches the leg's touchdown offset (continuous, monotone). The
|
|
1238
|
-
// pace is capped at 1.6x the planned one (when a gait change pulls the offset in, the paw
|
|
1239
|
-
// lands a little late instead of being snapped down) and eased over 40 ms (a change of pace
|
|
1240
|
-
// is not a kink in the paw's path); a paw still in the air after its offset is overdue
|
|
1241
|
-
// (signed remaining phase) and finishes at the capped pace
|
|
1242
|
-
if (p < leg.prevPhase - 0.5) leg.overdue = true; // the phase wrapped past the offset
|
|
1243
|
-
const pr = leg.overdue ? -p : 1 - p;
|
|
1244
|
-
const want = pr > 0 ? Math.min((1 - leg.w) / Math.max(pr / g.f, 1e-4), 1.6 / leg.swingNom) : 1.6 / leg.swingNom;
|
|
1245
|
-
leg.wPace = expTo(leg.wPace, want, dt, 0.04);
|
|
1246
|
-
leg.swingT = Math.min(1, leg.w + leg.wPace * dt);
|
|
1247
|
-
leg.swingDur = 1 / Math.max(leg.wPace, 1e-3); // (remaining time = (1 - w) swingDur)
|
|
1248
|
-
} else leg.swingT += dt / leg.swingDur;
|
|
1249
|
-
leg.w = clamp(leg.swingT, 0, 1);
|
|
1250
|
-
const remain = (1 - leg.w) * leg.swingDur;
|
|
1251
|
-
// (centred on the table's stance, not the reach-shortened one: a short stance asked for a short
|
|
1252
|
-
// lead, which kept the next stance short; the reach still caps the landing below)
|
|
1253
|
-
const stanceDur = lerp(D, Math.max(D, g.D), g.gallop * cfg.stride.gallop * (L.foot.type === 'unguligrade' ? 1 : 0)) / Math.max(g.f, 1e-3);
|
|
1254
|
-
// land half a stance sweep ahead of the girdle so the sweep is centred under it
|
|
1255
|
-
// ... but never further than the leg reaches from where its girdle is now (a landing out of
|
|
1256
|
-
// reach leaves the paw hovering above its plant). Continuous in speed: a switch to zero at
|
|
1257
|
-
// the 'moving' threshold made a swinging paw jump back 4 cm as the animal came to a halt.
|
|
1258
|
-
let lead = Math.min(v * stanceDur * 0.5 + (L.gz - L.cz) * smooth(0.05, 0.6, vn), L.leadMax * lerp(1, 1.5, g.gallop * cfg.stride.gallop * (L.foot.type === 'unguligrade' ? 1 : 0))) + g.lead * smooth(0, 0.3, vn);
|
|
1259
|
-
if (this.frame > 2) lead = Math.min(lead, Math.max(leg.reachF * (L.front ? 1 + (L.foot.type === 'unguligrade' ? 0.3 : 0) * g.gallop * cfg.stride.gallop : 1), 0));
|
|
1260
|
-
this.neutralContact(L, lead, remain, leg.land, leg);
|
|
1261
|
-
leg.landLocal.set(this._fl[0], 0, this._fl[1]);
|
|
1262
|
-
if (leg.swingT >= 1) {
|
|
1263
|
-
leg.state = 'stance';
|
|
1264
|
-
leg.fwdInit = false; // the paw's heading is taken at the first stance pose
|
|
1265
|
-
leg.windowUsed = false; // a new stance always gets its own lift-off window
|
|
1266
|
-
leg.plant.copy(leg.land);
|
|
1267
|
-
this.terrainN(leg.plant.x, leg.plant.z, leg.plantN);
|
|
1268
|
-
leg.stanceTime = 0;
|
|
1269
|
-
leg.u = 0;
|
|
1270
|
-
if (legGait > 0.3) this.emit('footstep', { foot: L.key, position: leg.plant.clone(), speed: v, strength: clamp(0.25 + vn / 12, 0, 1) * (leg.mode === 'idle' ? 0.5 : 1) });
|
|
1271
|
-
}
|
|
1272
|
-
}
|
|
1273
|
-
leg.prevPhase = p;
|
|
1274
|
-
}
|
|
1275
|
-
}
|
|
1276
|
-
|
|
1277
|
-
// Reach of a leg from its girdle's last pose, with the IK chain at <= 95 % of its length (the soft
|
|
1278
|
-
// reach limit starts at 95.5 %). Body-frame contact z (relative to L.cz, like the landing lead):
|
|
1279
|
-
// leg.reachF: furthest landing point (flat foot)
|
|
1280
|
-
// leg.reachB: furthest point behind at which a planted foot is still reached with the heel raised
|
|
1281
|
-
// to the gait's heel angle + 30 deg of toe-off (the paw rolls up about its toe)
|
|
1282
|
-
// leg.sweep = reachF - reachB: the stance sweep the leg can reach.
|
|
1283
|
-
reachSpan(leg) {
|
|
1284
|
-
const L = leg.def, fr = this.fr, J = this.J, F = fr.F;
|
|
1285
|
-
const G = L.front ? tv().copy(L.jShoulder).sub(J.chestMid).applyQuaternion(fr.qCh).add(fr.chestMid)
|
|
1286
|
-
: tv().copy(L.jHip).sub(J.lumbosacral).applyQuaternion(fr.qPel).add(fr.lumbo);
|
|
1287
|
-
const gzW = (G.x - fr.pos.x) * F.x + (G.z - fr.pos.z) * F.z; // (frames of the last pose)
|
|
1288
|
-
// (a jump's landing sag (touchDown, pose) is lifting the girdle back up by its offset: a foot set
|
|
1289
|
-
// down now lands under a girdle that much higher, a horse's forefoot at 96 % of its reach whose
|
|
1290
|
-
// elbow snapped from straightening to flexing as the body went over it, 86 rad/s*)
|
|
1291
|
-
const gh = this.terrainH(G.x, G.z) + (this.sag.on ? Math.min(0, L.front ? this.sag.F.x : this.sag.H.x) : 0);
|
|
1292
|
-
// sideways offset of the foot from the girdle joint (banked turns, narrow gallop stance)
|
|
1293
|
-
// (a swinging foot: its planned body-frame landing offset, not the predicted world landing point,
|
|
1294
|
-
// which moves with the lead computed from this reach and would feed back in a turn)
|
|
1295
|
-
const Lf = fr.Lf;
|
|
1296
|
-
const lat = leg.state === 'stance' ? (leg.plant.x - G.x) * Lf.x + (leg.plant.z - G.z) * Lf.z
|
|
1297
|
-
: leg.landLocal.x - ((G.x - fr.pos.x) * Lf.x + (G.z - fr.pos.z) * Lf.z);
|
|
1298
|
-
// (plantigrade hind: the heel / hock is the point the leg must reach, see toeOff)
|
|
1299
|
-
const plantH = L.plant && !L.front, Mb = plantH ? L.H : L.M;
|
|
1300
|
-
const R0 = 0.95 * (L.front ? L.Lh + L.Lr : plantH ? L.L1 + L.L2 : L.Av + L.L2), R2 = R0 * R0 - lat * lat;
|
|
1301
|
-
// (the reach X = sqrt(R2 - h^2) grows as a square root near full extension, where a girdle
|
|
1302
|
-
// bouncing by a centimetre moved it by decimetres: a horse's straight forelegs stand there, and a
|
|
1303
|
-
// trot braking through that height moved a swinging forefoot's landing 4 cm in one 240 Hz frame.
|
|
1304
|
-
// softRoot keeps its slope bounded, dX / dh <= h / (0.15 R0) ~ 7, and is exact wherever the
|
|
1305
|
-
// reach exceeds 0.15 R0: a wider blend, biased either way near full extension, misplaced the
|
|
1306
|
-
// 2-frame sprint landings of legs that stand nearly straight, a bear's and a horse's forelegs)
|
|
1307
|
-
const u0 = 0.0225 * R0 * R0;
|
|
1308
|
-
const reach = (beta, back) => {
|
|
1309
|
-
// IK target relative to the contact point with the heel raised by beta (M rolls up about T;
|
|
1310
|
-
// front: the wrist sits above the metacarpus, tilted back by a0 and forward by 0.6 beta)
|
|
1311
|
-
// (beta < 0 on a hoof: the fetlock sinks about the coffin joint C instead)
|
|
1312
|
-
const P0 = beta < 0 && L.C ? L.C : L.T;
|
|
1313
|
-
const my = Mb.y - P0.y, mz = Mb.z - P0.z, cb = Math.cos(beta), sb = Math.sin(beta);
|
|
1314
|
-
let dy = P0.y + my * cb - mz * sb, dz = P0.z + my * sb + mz * cb;
|
|
1315
|
-
if (L.front) { const cp = L.foot.couple; dy += L.Lmc * Math.cos(L.a0 - cp * beta); dz += L.Lmc * Math.sin(cp * beta - L.a0); }
|
|
1316
|
-
const h = G.y - gh - dy;
|
|
1317
|
-
const X = softRoot(R2 - h * h, u0);
|
|
1318
|
-
return gzW + (back ? -X : X) - dz - L.cz;
|
|
1319
|
-
};
|
|
1320
|
-
// (a hoof lands with the fetlock sinking under load: reached with most of that sink, which peaks
|
|
1321
|
-
// early in the stance while the foot is still ahead of the girdle)
|
|
1322
|
-
const sink = L.C && L.foot.sink ? -0.5 * L.foot.sink * DEG * smooth(0.05, 1, this.speed / this.cfg.sq) : 0;
|
|
1323
|
-
leg.reachF = reach(sink, false);
|
|
1324
|
-
// (a planted paw whose heel roll is rate-limited has not reached the gait's heel angle yet)
|
|
1325
|
-
const heel = leg.state === 'stance' && leg.stanceTime > 0 ? Math.min(this.gait.heel * DEG, leg.betaHeel + 0.1) : this.gait.heel * DEG;
|
|
1326
|
-
leg.reachB = reach(Math.min(100 * DEG, heel + 30 * DEG), true);
|
|
1327
|
-
leg.sweep = leg.reachF - leg.reachB;
|
|
1328
|
-
// leg.reachBL: the same with the heel the paw has now, its gait heel and the toe-off already under
|
|
1329
|
-
// way: the toe-off is low-passed and adds 11 % of its lift in a 60 Hz frame, so a stance of a few
|
|
1330
|
-
// frames (a gallop) cannot count on the other 30 deg; the lift-off tests this one (a bear's
|
|
1331
|
-
// galloping forefoot was dragged 2-4 % S through the last frame of its stance)
|
|
1332
|
-
leg.reachBL = reach(Math.min(100 * DEG, heel + Math.max(0, leg.toeS.x)), true);
|
|
1333
|
-
}
|
|
1334
|
-
|
|
1335
|
-
startSwing(leg, dur, g, idle) {
|
|
1336
|
-
this.worldToBody(leg.plant, leg.liftLocal);
|
|
1337
|
-
leg.liftN.copy(leg.plantN);
|
|
1338
|
-
leg.protStart = leg.state === 'stance' ? lerp(0.18, -0.25, leg.u) : -0.25;
|
|
1339
|
-
leg.state = 'swing';
|
|
1340
|
-
leg.track = false; leg.overdue = false;
|
|
1341
|
-
leg.mode = idle ? 'idle' : 'gait';
|
|
1342
|
-
leg.swingT = 0; leg.w = 0;
|
|
1343
|
-
leg.swingDur = leg.swingNom = Math.max(0.06, dur);
|
|
1344
|
-
leg.wPace = 1 / leg.swingDur;
|
|
1345
|
-
leg.heelLift = (idle ? 20 : g.heel) * DEG;
|
|
1346
|
-
// continue from the stance's actual heel angle, carpal angle and scapula protraction (no pop);
|
|
1347
|
-
// gait-only values: a posture blended over the gait (landing, lunge) is mixed in again on top
|
|
1348
|
-
leg.betaStart = leg.betaGaitLast ?? leg.betaLast ?? 0;
|
|
1349
|
-
leg.kStart = leg.def.front ? leg.kGait ?? leg.ft.k : 0;
|
|
1350
|
-
leg.toeS.reset(0); leg.betaAdd = 0; leg.sinkS.reset(0); leg.sinkLift = 0;
|
|
1351
|
-
leg.liftH = idle ? 0.05 * this.cfg.k : leg.def.front ? g.liftF : g.liftH;
|
|
1352
|
-
leg.land.copy(leg.plant);
|
|
1353
|
-
leg.landLocal.copy(leg.liftLocal);
|
|
1354
|
-
}
|
|
1355
|
-
|
|
1356
|
-
// jump support (called by the action layer)
|
|
1357
|
-
takeOff(vy, vh) {
|
|
1358
|
-
const a = this.air;
|
|
1359
|
-
a.active = true; a.t = 0; a.vy = vy; a.vh = vh;
|
|
1360
|
-
a.y0 = this.pos.y + this.lift;
|
|
1361
|
-
a.T = (2 * vy) / 9.81;
|
|
1362
|
-
this.speed = vh;
|
|
1363
|
-
this.state.grounded = false;
|
|
1364
|
-
this.emit('takeoff', {});
|
|
1365
|
-
}
|
|
1366
|
-
touchDown() {
|
|
1367
|
-
const a = this.air;
|
|
1368
|
-
a.active = false;
|
|
1369
|
-
// the landing (see pose): the girdles take over the body's sink speed at touchdown (m/s of this
|
|
1370
|
-
// individual, from its own jump height, so it needs no unit factor: the old kick of the girdle
|
|
1371
|
-
// springs by min(4, vy) x unit m/s over-kicked every species whose unit is not 1, a horse by
|
|
1372
|
-
// 2.6-3.1x its sink speed, and under-kicked a rat to 0.02x) and the flight's overshoot below its
|
|
1373
|
-
// landing height in this step; fore 0.9x, hind 1.1x (the hindquarters give more)
|
|
1374
|
-
const vLand = Math.min(Math.max(0, 9.81 * a.t - a.vy), 4 * this.cfg.sk) * (this.cfg.actions.jump?.give ?? 1);
|
|
1375
|
-
const over = Math.min(0, this.lift), sag = this.sag, cfg = this.cfg;
|
|
1376
|
-
sag.F.x += over; sag.F.v -= 0.9 * vLand;
|
|
1377
|
-
sag.H.x += over; sag.H.v -= 1.1 * vLand;
|
|
1378
|
-
// absorbed by critically damped springs (peak sag = speed / (e rate)) at the rate of the body's
|
|
1379
|
-
// springs scaled by 1 / sqrt(k) (dynamic similarity: an animal landing from the same height in
|
|
1380
|
-
// leg lengths sinks as far, in leg lengths), stiffer where that would sink a girdle more than 40 %
|
|
1381
|
-
// of its clearance above the torso resting on the ground (the section's bottom: a rat, jumping
|
|
1382
|
-
// two leg lengths high, would otherwise put its belly 3 cm down on the ground)
|
|
1383
|
-
const w0 = 17 / cfg.sk;
|
|
1384
|
-
const cF = 0.4 * Math.max(1e-3 * cfg.k, cfg.chest.y - cfg.chest.hh), cH = 0.4 * Math.max(1e-3 * cfg.k, cfg.pelvis.y - cfg.pelvis.hh);
|
|
1385
|
-
const wF = Math.max(w0, -sag.F.v / (Math.E * cF)), wH = Math.max(w0, -sag.H.v / (Math.E * cH));
|
|
1386
|
-
sag.wF = sag.on ? Math.max(sag.wF, wF) : wF; sag.wH = sag.on ? Math.max(sag.wH, wH) : wH;
|
|
1387
|
-
sag.on = true;
|
|
1388
|
-
this.lift = 0;
|
|
1389
|
-
this.state.grounded = true;
|
|
1390
|
-
// resume the gait where its phase is: legs inside their stance window plant now (at the place
|
|
1391
|
-
// the stride expects), the others finish a swing from where the tucked foot is
|
|
1392
|
-
const g = this.gait, moving = this.speed / this.cfg.sq > 0.06;
|
|
1393
|
-
for (const leg of this.legs) {
|
|
1394
|
-
const L = leg.def;
|
|
1395
|
-
const p = wrap01(this.phase - g.off[L.idx]), D = g.DL[L.idx];
|
|
1396
|
-
leg.prevPhase = p;
|
|
1397
|
-
if (leg.state === 'swing' && !(p < D * 0.85 && leg.w > 0.6)) {
|
|
1398
|
-
// keep the running swing, re-timed to the stride
|
|
1399
|
-
leg.windowUsed = true;
|
|
1400
|
-
continue;
|
|
1401
|
-
}
|
|
1402
|
-
if (!moving || p < D * 0.85) {
|
|
1403
|
-
const u = moving ? p / D : 0.3;
|
|
1404
|
-
const half = moving ? Math.min(this.speed * D / g.f * 0.5, L.leadMax) : 0;
|
|
1405
|
-
this.neutralContact(L, half * (1 - 2 * u), 0, leg.plant, leg);
|
|
1406
|
-
this.terrainN(leg.plant.x, leg.plant.z, leg.plantN);
|
|
1407
|
-
leg.state = 'stance'; leg.stanceTime = 0; leg.u = u; leg.windowUsed = false; leg.fwdInit = false;
|
|
1408
|
-
this.emit('footstep', { foot: L.key, position: leg.plant.clone(), speed: this.speed, strength: 1 });
|
|
1409
|
-
} else {
|
|
1410
|
-
leg.windowUsed = true;
|
|
1411
|
-
leg.betaLast = 0;
|
|
1412
|
-
this.startSwing(leg, Math.max(0.08, (1 - p) / g.f), g, false);
|
|
1413
|
-
}
|
|
1414
|
-
}
|
|
1415
|
-
this.emit('land', { position: this.pos.clone(), speed: this.speed });
|
|
1416
|
-
this.actionsLayer.onLand();
|
|
1417
|
-
}
|
|
1418
|
-
|
|
1419
|
-
// current contact point (on / above the ground) of a leg in the gait. Swing paths are
|
|
1420
|
-
// interpolated in body space so the paw moves naturally relative to the body at any speed.
|
|
1421
|
-
contactPoint(leg, out) {
|
|
1422
|
-
if (leg.state === 'stance') return out.copy(leg.plant);
|
|
1423
|
-
const w = leg.w, front = leg.def.front, g = this.gait;
|
|
1424
|
-
const a = leg.liftLocal, b = leg.landLocal;
|
|
1425
|
-
const s = smooth(0, 1, w * 1.05 - 0.02);
|
|
1426
|
-
let z = lerp(a.z, b.z, s);
|
|
1427
|
-
const rt = leg.def.foot.retract || 0;
|
|
1428
|
-
if (rt > 0 && leg.mode === 'gait') {
|
|
1429
|
-
// swing retraction (feet.*.retract 0..1): a Hermite path whose end tangents cancel the body's
|
|
1430
|
-
// speed, so the foot leaves and meets the ground at (nearly) zero ground speed instead of
|
|
1431
|
-
// stopping dead at touchdown; small fast animals otherwise pop in their 2-3 frame stances
|
|
1432
|
-
const m = -rt * Math.min(this.speed * leg.swingDur, 1.5 * Math.abs(b.z - a.z) + 0.02 * this.cfg.k);
|
|
1433
|
-
const w2 = w * w, w3 = w2 * w;
|
|
1434
|
-
z = (2 * w3 - 3 * w2 + 1) * a.z + (w3 - 2 * w2 + w) * m + (-2 * w3 + 3 * w2) * b.z + (w3 - w2) * m;
|
|
1435
|
-
}
|
|
1436
|
-
const tuck = leg.mode === 'gait' ? g.gallop : 0;
|
|
1437
|
-
if (front) z -= 0.12 * this.cfg.k * tuck * Math.sin(Math.PI * clamp(w / 0.55, 0, 1)) * (1 - w);
|
|
1438
|
-
// long-footed legs (hooves): the foot trails behind its knee in swing, so its path bows back
|
|
1439
|
-
const sb = leg.def.foot.swingBack;
|
|
1440
|
-
if (sb && leg.mode === 'gait') z -= sb * this.cfg.s * Math.sin(Math.PI * w);
|
|
1441
|
-
this.bodyToWorld(lerp(a.x, b.x, s), 0, z, this.heading, this.pos, out);
|
|
1442
|
-
// lift profile: fronts rise early and descend while reaching, hinds peak mid swing. 1 - (1 - w)^q
|
|
1443
|
-
// rather than w^p: the same early rise, but a finite lift-off speed (w^p, p < 1, has an infinite
|
|
1444
|
-
// slope at w = 0: the paw jumped ~6 cm in the first 1/240 s of a gallop swing)
|
|
1445
|
-
const wp = 1 - Math.pow(1 - w, front ? 1 / lerp(0.8, 0.55, tuck) : 1 / 0.85);
|
|
1446
|
-
const bump = Math.pow(Math.sin(Math.PI * wp), 1.1);
|
|
1447
|
-
const gh = this.terrainH(out.x, out.z);
|
|
1448
|
-
const y0 = lerp(a.y, gh, smooth(0, 0.6, w));
|
|
1449
|
-
// in flight the feet rise with the body (from zero at take-off), not from the ground
|
|
1450
|
-
let y = y0 + leg.liftH * bump + (this.air.active ? Math.max(0, this.lift) : 0);
|
|
1451
|
-
// never fold the foot up into the girdle (the chain direction would spin as it passes)
|
|
1452
|
-
const L = leg.def, fr = this.fr;
|
|
1453
|
-
const gy = (front ? fr.shC.y : fr.hipC.y) - (front ? 0.62 : 0.55) * L.len;
|
|
1454
|
-
y = smin(y, gy, 0.05 * this.cfg.k);
|
|
1455
|
-
out.y = Math.max(y, gh + 0.004 * bump);
|
|
1456
|
-
return out;
|
|
1457
|
-
}
|
|
1458
|
-
|
|
1459
|
-
// ----------------------------------------------------------------- pose
|
|
1460
|
-
pose(dt) {
|
|
1461
|
-
const cfg = this.cfg, g = this.gait, P = this.P, J = this.J;
|
|
1462
|
-
const v = this.speed, vn = v / cfg.sq, s = cfg.k;
|
|
1463
|
-
const h = this.heading;
|
|
1464
|
-
const fr = this.fr;
|
|
1465
|
-
this.forward(h, fr.F); this.left(h, fr.Lf); fr.pos.copy(this.pos); this._poseH = h;
|
|
1466
|
-
fr.qHeading.setFromAxisAngle(UP, h);
|
|
1467
|
-
const gw8 = P.gaitW; // weight of the locomotion styling on the body
|
|
1468
|
-
|
|
1469
|
-
// --- girdle loads -> vertical bounce, from the gait phase so re-timed steps never jolt the body
|
|
1470
|
-
// (the load follows each foot's own stance progress u, continuous from touchdown (0) to lift-off
|
|
1471
|
-
// (1): with the bounce applied after the body springs, a load read from the gait phase jumped
|
|
1472
|
-
// whenever the reach shortened a stance)
|
|
1473
|
-
let loadF = 0, loadH = 0;
|
|
1474
|
-
for (const leg of this.legs) {
|
|
1475
|
-
let sn;
|
|
1476
|
-
if (cfg.stride.bobSpring) { const pl = wrap01(this.phase - g.off[leg.def.idx]), D = g.DL[leg.def.idx]; sn = pl < D ? Math.sin(Math.PI * pl / D) : 0; }
|
|
1477
|
-
else {
|
|
1478
|
-
sn = leg.state === 'stance' && leg.mode === 'gait' ? Math.sin(Math.PI * clamp(leg.u, 0, 1)) : 0;
|
|
1479
|
-
// (gallop: from the table's stance window, not the stance the reach allowed: the forequarters
|
|
1480
|
-
// come down when the forelegs should bear the weight, and that drop is what lets them stay down)
|
|
1481
|
-
if (g.gallop > 0.01 && cfg.stride.gallop > 0) { const pl = wrap01(this.phase - g.off[leg.def.idx]); const sg = pl < g.D ? Math.sin(Math.PI * pl / g.D) : 0; sn = lerp(sn, sg, g.gallop * cfg.stride.gallop); }
|
|
1482
|
-
}
|
|
1483
|
-
if (leg.def.front) loadF = Math.max(loadF, sn); else loadH = Math.max(loadH, sn);
|
|
1484
|
-
}
|
|
1485
|
-
const lf = cfg.stride.bobSpring ? 1 : 1 - Math.exp(-dt * lerp(28, 21, g.gallop));
|
|
1486
|
-
this._loadF = (this._loadF ?? loadF) + (loadF - (this._loadF ?? loadF)) * lf; loadF = this._loadF;
|
|
1487
|
-
this._loadH = (this._loadH ?? loadH) + (loadH - (this._loadH ?? loadH)) * lf; loadH = this._loadH;
|
|
1488
|
-
const moving = vn > 0.06 || Math.abs(this.yawRate) > 0.1;
|
|
1489
|
-
this._bobGate = expTo(this._bobGate ?? 0, this.air.active ? 0 : 1, dt, 0.05);
|
|
1490
|
-
const mv = smooth(0.03, 0.4, vn + Math.abs(this.yawRate) * 0.3) * this._bobGate;
|
|
1491
|
-
// (spring-mass: lowest at mid stance under the girdle's load, up in the flight; mostly downward, so
|
|
1492
|
-
// a girdle is not raised away from feet landing or pushing off at the ends of their reach)
|
|
1493
|
-
// (stride.bobSpring: the bounce goes through the girdle springs as a target instead, smoothed and
|
|
1494
|
-
// late: hoppers, whose hops were tuned on it, land their flat hind feet clear of the ground with it)
|
|
1495
|
-
const bK = cfg.stride.bob, bO = cfg.stride.bobSpring ? 0.6 : 0.3;
|
|
1496
|
-
// (at the gallop the forequarters drop onto the landing forelegs, much more than a trot's bounce:
|
|
1497
|
-
// the drop is what lets a galloping foreleg sweep its long stance)
|
|
1498
|
-
// (paws: half the drop: their flexible backs bend into the stride, and more stretched the neck)
|
|
1499
|
-
const galB = 1 + (cfg.legs[0].foot.type === 'unguligrade' ? 1 : 0.5) * cfg.stride.gallop;
|
|
1500
|
-
let bobS = g.bobF * (bO - loadF) * 2 * bK * mv * gw8 * lerp(1, galB, g.gallop);
|
|
1501
|
-
let bobH = g.bobH * (bO - loadH) * 2 * bK * mv * gw8 * lerp(1, 1 + (galB - 1) * 0.25, g.gallop);
|
|
1502
|
-
const bobT = cfg.stride.bobSpring;
|
|
1503
|
-
const crouch = clamp((this.input.crouch || 0) + cfg.stance.crouch, 0, 1);
|
|
1504
|
-
|
|
1505
|
-
// terrain under the girdles (max of two samples along the body when resting on the ground)
|
|
1506
|
-
const shG = this.bodyToWorld(0, 0, cfg.zS, h, this.pos, tv());
|
|
1507
|
-
const hpG = this.bodyToWorld(0, 0, cfg.zH, h, this.pos, tv());
|
|
1508
|
-
let tS = this.terrainH(shG.x, shG.z), tH = this.terrainH(hpG.x, hpG.z);
|
|
1509
|
-
if (P.groundW > 0.01) {
|
|
1510
|
-
const e = 0.12 * s, sh = Math.sin(h), ch = Math.cos(h);
|
|
1511
|
-
const m = (x, z) => Math.max(this.terrainH(x + sh * e, z + ch * e), this.terrainH(x - sh * e, z - ch * e), this.terrainH(x + ch * e * 0.6, z - sh * e * 0.6), this.terrainH(x - ch * e * 0.6, z + sh * e * 0.6));
|
|
1512
|
-
tS = lerp(tS, Math.max(tS, m(shG.x, shG.z)), P.groundW);
|
|
1513
|
-
tH = lerp(tH, Math.max(tH, m(hpG.x, hpG.z)), P.groundW);
|
|
1514
|
-
}
|
|
1515
|
-
// resting heights (girdle joint above the ground when the torso touches it), for the current roll
|
|
1516
|
-
const dl = tv(0, -1, 0).applyQuaternion(tqc(this.qBprev).invert());
|
|
1517
|
-
const restF = restHeight(cfg.chest, cfg.yS, dl), restH = restHeight(cfg.pelvis, cfg.yH, dl);
|
|
1518
|
-
const dropF = P.dropF + crouch * 0.3, dropH = P.dropH + crouch * 0.18;
|
|
1519
|
-
// (the gait's bounce is added after the body's height and pitch springs, below: through them, a
|
|
1520
|
-
// bounce at twice the stride frequency came out at a quarter of its size and a third of a cycle
|
|
1521
|
-
// late, the body highest as a trot's feet landed and sinking through the stance)
|
|
1522
|
-
let yShT = tS - g.drop * gw8 - dropF * (cfg.yS - restF) + (bobT ? bobS : 0);
|
|
1523
|
-
let yHpT = tH - g.drop * gw8 - dropH * (cfg.yH - restH) + (bobT ? bobH : 0);
|
|
1524
|
-
if (bobT) { bobS = 0; bobH = 0; }
|
|
1525
|
-
// braking and pushing off: hard braking sits the body back on its haunches (hindquarters down,
|
|
1526
|
-
// forequarters a little up, the head raised: solveHead), pushing off lowers it into the effort;
|
|
1527
|
-
// through the girdle springs, so the body rocks forward and settles as the braking ends
|
|
1528
|
-
const aFw = this.accel.x * fr.F.x + this.accel.z * fr.F.z;
|
|
1529
|
-
this._aF = expTo(this._aF ?? 0, clamp(aFw, -20 * cfg.sk, 20 * cfg.sk), dt, 0.12);
|
|
1530
|
-
this.brake = clamp(-this._aF / 9.81, 0, 0.9) * gw8 * smooth(0.3, 1.5, vn + 0.5) * cfg.stride.brake;
|
|
1531
|
-
const push = clamp(this._aF / 9.81, 0, 0.6) * gw8 * cfg.stride.brake;
|
|
1532
|
-
yHpT -= (0.12 * this.brake + 0.04 * push) * (cfg.yH - restH);
|
|
1533
|
-
yShT += (0.025 * this.brake - 0.04 * push) * (cfg.yS - restF);
|
|
1534
|
-
if (P.frontHW > 0) yShT = lerp(yShT, tS + P.frontH - cfg.yS, P.frontHW); // (sit: straight forelegs)
|
|
1535
|
-
// head reach: lower the forequarters when the mouth target is below what the neck can reach
|
|
1536
|
-
if (P.headReach > 0.01 && P.headW > 0.01) {
|
|
1537
|
-
const nbY = yShT + J.neckBase.y;
|
|
1538
|
-
// (the neck reaches 0.8 of its length down, less when the target is also far ahead of it)
|
|
1539
|
-
const dh = Math.hypot(P.headPos.x - fr.neckBase.x, P.headPos.z - fr.neckBase.z), Ln = cfg.neckLen;
|
|
1540
|
-
const need = nbY - (P.headPos.y + Math.min(0.8 * Ln, Math.sqrt(Math.max(0, 0.85 * Ln * Ln - dh * dh))));
|
|
1541
|
-
// (never lower than the chest resting on the ground)
|
|
1542
|
-
if (need > 0) yShT = Math.max(yShT - need * P.headReach, Math.min(yShT, tS - (cfg.yS - restF)));
|
|
1543
|
-
}
|
|
1544
|
-
|
|
1545
|
-
// banked turn: the body leans over its feet, rolling about the ground under it (buildBody), so the
|
|
1546
|
-
// girdles come down by their height above the ground x (1 - cos lean) and the legs lean with it,
|
|
1547
|
-
// keeping their length. Restoring the upright girdle heights (as the body frame did) sheared the
|
|
1548
|
-
// body sideways over its feet instead: at a sprint turn's 34 deg lean a cheetah's shoulders sat
|
|
1549
|
-
// 0.36 m inside its feet at full height, and its forelegs could not reach the ground (4 stances in
|
|
1550
|
-
// 6 s of turning). In the targets, so the girdle springs and the reach limits below take it
|
|
1551
|
-
const bL = this.bank.x * (1 - P.groundW), cbL = Math.cos(bL), sbL = Math.sin(bL);
|
|
1552
|
-
yShT = tS + (yShT + cfg.yS - tS) * cbL - cfg.yS; yHpT = tH + (yHpT + cfg.yH - tH) * cbL - cfg.yH;
|
|
1553
|
-
// leg reach limits: lower a girdle whose stance feet are too far away (smooth minimum, faded in
|
|
1554
|
-
// and out over the first / last bit of stance so it never switches on abruptly)
|
|
1555
|
-
const lgw = P.legGait;
|
|
1556
|
-
if (lgw > 0.01 && !this.air.active) {
|
|
1557
|
-
for (const leg of this.legs) {
|
|
1558
|
-
if (leg.state !== 'stance') continue;
|
|
1559
|
-
const L = leg.def, c = leg.plant;
|
|
1560
|
-
// (the girdle joint where the lean carries it: in toward the turn by its upright height x sin
|
|
1561
|
-
// lean, and the outside one up by its half width x sin lean)
|
|
1562
|
-
const H0 = (L.front ? yShT + cfg.yS - tS : yHpT + cfg.yH - tH) / cbL;
|
|
1563
|
-
const jw = this.bodyToWorld(L.gx * cbL - H0 * sbL, 0, L.gz, h, this.pos, tv());
|
|
1564
|
-
const dxz = Math.hypot(jw.x - c.x, jw.z - c.z);
|
|
1565
|
-
const jy = L.front ? cfg.yS : cfg.yH;
|
|
1566
|
-
let maxY = Math.sqrt(Math.max(0.01 * s * s, L.reach * L.reach - dxz * dxz)) + c.y + 0.03 * s - L.gx * sbL - jy;
|
|
1567
|
-
const floor = (L.front ? tS : tH) - g.drop - crouch * 0.14 * s - lerp(0.025, 0.07, g.gallop) * s - (L.front ? dropF * (cfg.yS - restF) : dropH * (cfg.yH - restH));
|
|
1568
|
-
maxY = Math.max(maxY, floor);
|
|
1569
|
-
const w = smooth(0, 0.12, leg.u) * (1 - smooth(0.9, 1.0, leg.u) * 0.5) * lgw;
|
|
1570
|
-
const cur = L.front ? yShT : yHpT;
|
|
1571
|
-
const lim = lerp(cur, Math.min(cur, maxY), w);
|
|
1572
|
-
if (L.front) yShT = smin(yShT, lim + 0.004 * s, 0.03 * s); else yHpT = smin(yHpT, lim + 0.004 * s, 0.03 * s);
|
|
1573
|
-
}
|
|
1574
|
-
}
|
|
1575
|
-
const gw = moving ? 17 : 9;
|
|
1576
|
-
const om = gw * P.bodyOmega;
|
|
1577
|
-
let ySh = this.shY.step(yShT, om, dt);
|
|
1578
|
-
let yHp = this.hipY.step(yHpT, om, dt);
|
|
1579
|
-
const sag = this.sag;
|
|
1580
|
-
if (sag.on) {
|
|
1581
|
-
// jump landing (touchDown): the girdles start at the flight's height (its overshoot below the
|
|
1582
|
-
// landing height in the touchdown frame) and keep moving down at the body's sink speed, then
|
|
1583
|
-
// settle on critically damped springs
|
|
1584
|
-
ySh += sag.F.x; yHp += sag.H.x;
|
|
1585
|
-
sag.F.step(0, sag.wF, dt); sag.H.step(0, sag.wH, dt);
|
|
1586
|
-
if (Math.abs(sag.F.x) + Math.abs(sag.H.x) + Math.abs(sag.F.v) / sag.wF + Math.abs(sag.H.v) / sag.wH < 1e-5 * s) { sag.on = false; sag.F.reset(0); sag.H.reset(0); }
|
|
1587
|
-
}
|
|
1588
|
-
|
|
1589
|
-
// body frame
|
|
1590
|
-
const zS = cfg.zS, zH = cfg.zH;
|
|
1591
|
-
const pitchT = Math.atan2(ySh - yHp, zS - zH);
|
|
1592
|
-
const pitch = this.pitch.step(pitchT, 16 * P.bodyOmega, dt) + P.pitch + Math.atan2(bobS - bobH, zS - zH);
|
|
1593
|
-
const baseY = this.height.step((ySh * -zH + yHp * zS) / (zS - zH), 20 * P.bodyOmega, dt) + (bobS * -zH + bobH * zS) / (zS - zH);
|
|
1594
|
-
// (leaning into a turn is dynamic balance: a walking quadruped stands on three legs and turns level,
|
|
1595
|
-
// a trot leans a little and a canter or gallop fully, by the Froude number v^2 / (g leg): the walk
|
|
1596
|
-
// leaned up to 12 deg into its turns)
|
|
1597
|
-
let bankT = clamp(-Math.atan2(v * this.yawRate, 9.81) * 0.85 * cfg.bankScale, -0.6, 0.6) * gw8 * smooth(0.5, 2.5, v * v / (9.81 * cfg.legLen));
|
|
1598
|
-
// stride roll (the body rolls toward the supporting side once a stride at the walk and trot), added
|
|
1599
|
-
// after the bank spring, which filtered it to a third: hoofed trotters (no lateral spine wave in
|
|
1600
|
-
// their table) roll too, a little less
|
|
1601
|
-
const rollS = Math.sin(TAU * (this.phase + 0.1)) * (g.lat > 0 ? 0.03 : 0.018) * (1 - g.gallop) * gw8 * mv * cfg.stride.roll;
|
|
1602
|
-
if (P.groundW > 0.01) {
|
|
1603
|
-
// lie along the terrain's cross slope
|
|
1604
|
-
const nrm = this.terrainN(this.pos.x, this.pos.z, tv());
|
|
1605
|
-
bankT += P.groundW * Math.atan2(nrm.dot(fr.Lf), nrm.y);
|
|
1606
|
-
}
|
|
1607
|
-
const bank = this.bank.step(bankT, 10, dt) + P.roll + rollS;
|
|
1608
|
-
|
|
1609
|
-
// spine bending
|
|
1610
|
-
let flexT = 0, latT = 0, latTH = 0;
|
|
1611
|
-
if (moving && !this.air.active) {
|
|
1612
|
-
flexT = g.flex * Math.cos(TAU * (this.phase - 0.95)) * smooth(cfg.flexBlend[0], cfg.flexBlend[1], vn) * gw8;
|
|
1613
|
-
latT += g.lat * Math.sin(TAU * this.phase) * (1 - g.gallop) * gw8;
|
|
1614
|
-
}
|
|
1615
|
-
latTH = latT;
|
|
1616
|
-
{
|
|
1617
|
-
// turning: each girdle faces the way it moves. The body turns about its turn centre (turnCentre),
|
|
1618
|
-
// so a girdle l ahead of it moves at atan(yaw rate x l / v) off the heading (into the turn), one
|
|
1619
|
-
// behind it as far out of it: the chest turns into the turn and the pelvis stays back along the
|
|
1620
|
-
// track, which in a steady turn is the bend that lays both girdles on one circle (girdle span /
|
|
1621
|
-
// radius) and in a changing one follows the track's own lag. Soft-limited by the spine's
|
|
1622
|
-
// flexibility (cfg.bendMax; latT / latTH are the front / hind bends of buildBody, which turn the
|
|
1623
|
-
// chest by 0.4 and the pelvis by 0.6 of theirs). It was 0.55 x yaw rate / (0.35 v + 0.6) for both
|
|
1624
|
-
// halves, saturated at 0.45 rad in every walking turn of every species (a horse bent 26 deg)
|
|
1625
|
-
const zc = this.turnCentre(), vE = Math.max(v, 0.6 * cfg.sk), w = this.yawRate;
|
|
1626
|
-
let cS = Math.atan2(w * Math.max(0, cfg.zS - zc), vE), cH = Math.atan2(w * Math.max(0, zc - cfg.zH), vE);
|
|
1627
|
-
const tot = Math.abs(cS) + Math.abs(cH), bm = cfg.bendMax;
|
|
1628
|
-
if (tot > 1e-6) { const k = bm * Math.tanh(tot / bm) / tot; cS *= k; cH *= k; }
|
|
1629
|
-
latT += cS / 0.4 * gw8; latTH += cH / 0.6 * gw8;
|
|
1630
|
-
}
|
|
1631
|
-
const flex = this.flexS.step(flexT, 40, dt) + P.flex;
|
|
1632
|
-
const lat = this.latS.step(latT, 18, dt) + P.bend;
|
|
1633
|
-
const latH = this.latH.step(latTH, 18, dt) + P.bend;
|
|
1634
|
-
// (the locomotion's own bend, without a posture's: the feet follow it, not a sleeping curl)
|
|
1635
|
-
this._latF = this.latS.x; this._latH = this.latH.x;
|
|
1636
|
-
fr.comp = 0.11 * s * Math.max(-0.5, flex);
|
|
1637
|
-
// breathing
|
|
1638
|
-
const hb = cfg.breath;
|
|
1639
|
-
this.breath += dt * TAU * lerp(hb.rate, hb.pant ? hb.pantRate : hb.rate * 2.5, this.heat) * P.breathRate;
|
|
1640
|
-
const bAmp = lerp(hb.amp, hb.pant ? hb.pantAmp : hb.amp * 2, this.heat) * P.breathAmp;
|
|
1641
|
-
fr.bs = 1 + Math.sin(this.breath) * bAmp;
|
|
1642
|
-
|
|
1643
|
-
// body origin: ground point + height + posture offsets (lunge, jump flight)
|
|
1644
|
-
const origin = fr.origin.set(this.pos.x, baseY + this.lift + P.lift, this.pos.z);
|
|
1645
|
-
origin.addScaledVector(fr.F, P.fwd * s).addScaledVector(fr.Lf, P.side * s);
|
|
1646
|
-
this.buildBody(origin, pitch, bank, flex, lat, latH);
|
|
1647
|
-
// keep the girdles where the legs want them while the spine arches
|
|
1648
|
-
// (in a banked turn the girdle targets already hold the leaned heights, see above)
|
|
1649
|
-
const eS = ySh + bobS + this.lift + P.lift + cfg.yS - fr.shC.y, eH = yHp + bobH + this.lift + P.lift + cfg.yH - fr.hipC.y;
|
|
1650
|
-
origin.y += (eS + eH) * 0.5;
|
|
1651
|
-
let pitch2 = pitch + Math.atan2(eS - eH, zS - zH);
|
|
1652
|
-
this.buildBody(origin, pitch2, bank, flex, lat, latH);
|
|
1653
|
-
// resting on the ground: lift / tilt the torso so its chest, middle and pelvis sections sit on
|
|
1654
|
-
// the terrain under them instead of in it
|
|
1655
|
-
// (while the body is down: the posture's ground weight fades as soon as it stands up, and a cow
|
|
1656
|
-
// rising hind end first, its chest still down, lay 53 mm in the ground half way up)
|
|
1657
|
-
const wLie = Math.min(1, Math.max(P.groundW, P.dropF, P.dropH));
|
|
1658
|
-
for (let it = 0; it < 2 && wLie > 0.01; it++) {
|
|
1659
|
-
// sections along the torso: [section, bind z, frame origin, frame joint, frame quaternion]
|
|
1660
|
-
let dF = 0, dH = 0;
|
|
1661
|
-
for (let k = 0; k < 6; k++) {
|
|
1662
|
-
const sc = k < 2 ? cfg.chest : k === 2 ? cfg.mid : k === 5 ? cfg.rump : cfg.pelvis;
|
|
1663
|
-
const z = k === 0 ? zS : k === 1 ? J.chestMid.z : k === 2 ? J.thoraxRear.z : k === 3 ? J.lumbosacral.z : k === 4 ? zH : cfg.rump.z;
|
|
1664
|
-
const pen = k < 2 ? this.sectionPen(sc, z, fr.chestMid, J.chestMid, fr.qCh, k)
|
|
1665
|
-
: k === 2 ? this.sectionPen(sc, z, fr.root, J.thoraxRear, fr.qS3, k)
|
|
1666
|
-
: this.sectionPen(sc, z, fr.lumbo, J.lumbosacral, fr.qPel, k);
|
|
1667
|
-
if (pen <= 0) continue;
|
|
1668
|
-
// least-norm girdle lifts that raise this point by pen
|
|
1669
|
-
const a = clamp((zS - z) / (zS - zH), 0, 1), nn = (1 - a) * (1 - a) + a * a;
|
|
1670
|
-
dF = Math.max(dF, pen * (1 - a) / nn); dH = Math.max(dH, pen * a / nn);
|
|
1671
|
-
}
|
|
1672
|
-
dF *= wLie; dH *= wLie;
|
|
1673
|
-
if (dF > 1e-5 || dH > 1e-5) {
|
|
1674
|
-
origin.y += (dF * -zH + dH * zS) / (zS - zH);
|
|
1675
|
-
pitch2 += Math.atan2(dF - dH, zS - zH);
|
|
1676
|
-
this.buildBody(origin, pitch2, bank, flex, lat, latH);
|
|
1677
|
-
}
|
|
1678
|
-
}
|
|
1679
|
-
this.qBprev.copy(fr.qB);
|
|
1680
|
-
const bs = fr.bs, l2 = J.thoraxRear.distanceTo(J.lumbarMid);
|
|
1681
|
-
this.setBoneQ('spine3', fr.root, fr.qS3, this._scale.set(bs, 1, 1 + (bs - 1) * 0.6));
|
|
1682
|
-
this.setBoneQ('chest', fr.chestMid, fr.qCh, this._scale.set(1 + (bs - 1) * 0.6, 1, 1 + (bs - 1) * 0.4));
|
|
1683
|
-
this.setBoneQ('spine2', fr.lumbarMid, fr.qS2, this._scale.set(1 + (bs - 1) * 0.5, 1 - fr.comp / l2, 1 + (bs - 1) * 0.5));
|
|
1684
|
-
this.setBoneQ('spine1', fr.lumbo, fr.qS1, ONE);
|
|
1685
|
-
this.setBoneQ('pelvis', fr.lumbo, fr.qPel, ONE);
|
|
1686
|
-
|
|
1687
|
-
// legs
|
|
1688
|
-
const lodSkip = this.lod === 2 && (this.frame & 1) === 1;
|
|
1689
|
-
for (let i = 0; i < 4; i++) {
|
|
1690
|
-
const leg = this.legs[i];
|
|
1691
|
-
// (only swinging legs whose last three solves were exactly two frames apart)
|
|
1692
|
-
const f = this.frame, sf = leg.sf;
|
|
1693
|
-
if (lodSkip && sf[2] === f - 1 && sf[1] === f - 3 && sf[0] === f - 5 && leg.state === 'swing' && leg.w < 0.9 && this.P.legGait > 0.99) this.extrapolateLeg(leg);
|
|
1694
|
-
else this.solveLeg(leg, dt);
|
|
1695
|
-
}
|
|
1696
|
-
this.solveHead(dt);
|
|
1697
|
-
if (cfg.tailN) this.solveTail(dt);
|
|
1698
|
-
for (const x of this.extraBones) x.bone.matrixWorld.multiplyMatrices(x.parent.matrixWorld, x.rel);
|
|
1699
|
-
if (this.hooks.pose) this.hooks.pose(this, dt);
|
|
1700
|
-
}
|
|
1701
|
-
|
|
1702
|
-
// spine chain from an origin (ground point + height), pitch, bank, flexion and lateral bend
|
|
1703
|
-
buildBody(origin, pitchA, bank, flex, lat, latH = lat) {
|
|
1704
|
-
const fr = this.fr, J = this.J, cfg = this.cfg;
|
|
1705
|
-
const bend = this._bend;
|
|
1706
|
-
fr.qB.setFromAxisAngle(UP, this.heading).multiply(tqa(X1, -pitchA)).multiply(tqa(Z1, bank));
|
|
1707
|
-
fr.root.copy(J.thoraxRear).applyQuaternion(fr.qB).add(origin);
|
|
1708
|
-
bend(fr.qS3.copy(fr.qB), flex * 0.07, lat * 0.2, true);
|
|
1709
|
-
fr.chestMid.copy(J.chestMid).sub(J.thoraxRear).applyQuaternion(fr.qS3).add(fr.root);
|
|
1710
|
-
bend(fr.qCh.copy(fr.qS3), flex * 0.045, lat * 0.2, true);
|
|
1711
|
-
fr.neckBase.copy(J.neckBase).sub(J.chestMid).applyQuaternion(fr.qCh).add(fr.chestMid);
|
|
1712
|
-
bend(fr.qS2.copy(fr.qB), flex * 0.1, latH * 0.15, false);
|
|
1713
|
-
const l2 = J.thoraxRear.distanceTo(J.lumbarMid);
|
|
1714
|
-
fr.lumbarMid.copy(J.lumbarMid).sub(J.thoraxRear).multiplyScalar(1 - fr.comp / l2).applyQuaternion(fr.qS2).add(fr.root);
|
|
1715
|
-
bend(fr.qS1.copy(fr.qS2), flex * 0.35, latH * 0.2, false);
|
|
1716
|
-
fr.lumbo.copy(J.lumbosacral).sub(J.lumbarMid).applyQuaternion(fr.qS1).add(fr.lumbarMid);
|
|
1717
|
-
bend(fr.qPel.copy(fr.qS1), flex * 0.35, latH * 0.25, false);
|
|
1718
|
-
fr.tailBase.copy(J.tailBase).sub(J.lumbosacral).applyQuaternion(fr.qPel).add(fr.lumbo);
|
|
1719
|
-
fr.hipC.set(0, cfg.yH, cfg.zH).sub(J.lumbosacral).applyQuaternion(fr.qPel).add(fr.lumbo);
|
|
1720
|
-
fr.shC.set(0, cfg.yS, cfg.zS).sub(J.chestMid).applyQuaternion(fr.qCh).add(fr.chestMid);
|
|
1721
|
-
}
|
|
1722
|
-
|
|
1723
|
-
// how far a torso section (bind centre (0, sec.y, z), carried by frame (o, jo, q)) dips into the terrain
|
|
1724
|
-
sectionPen(sec, z, o, jo, q, ks) {
|
|
1725
|
-
const c = tv(0, sec.y, z).sub(jo).applyQuaternion(q).add(o);
|
|
1726
|
-
const dl = tv(0, -1, 0).applyQuaternion(tqc(q).invert());
|
|
1727
|
-
let sup = Math.sqrt((sec.hh * dl.y) ** 2 + (sec.hw * dl.x) ** 2), hw = sec.hw;
|
|
1728
|
-
// (or the skin's own support there, where it reaches further: cfg.secSup)
|
|
1729
|
-
const T = ks !== undefined && this.cfg.secSup ? this.cfg.secSup[ks] : null;
|
|
1730
|
-
if (T && T[0] > -Infinity) {
|
|
1731
|
-
const rxy = Math.hypot(dl.x, dl.y), u = clamp((Math.atan2(dl.x, -dl.y) / Math.PI + 0.5) * (SUP_N - 1), 0, SUP_N - 1);
|
|
1732
|
-
const j = Math.min(SUP_N - 2, Math.floor(u)), f = u - j;
|
|
1733
|
-
sup = Math.max(sup, rxy * (T[j] + (T[j + 1] - T[j]) * f));
|
|
1734
|
-
hw = Math.max(hw, T[0], T[SUP_N - 1]);
|
|
1735
|
-
}
|
|
1736
|
-
// the lowest point moves sideways when rolled: sample the terrain there too
|
|
1737
|
-
const side = tv(1, 0, 0).applyQuaternion(q);
|
|
1738
|
-
const k = hw * clamp(-side.y * 1.2, -1, 1);
|
|
1739
|
-
const gx = c.x + side.x * k, gz = c.z + side.z * k;
|
|
1740
|
-
const t = Math.max(this.terrainH(c.x, c.z), this.terrainH(gx, gz));
|
|
1741
|
-
return Math.max(0, t - (c.y - sup));
|
|
1742
|
-
}
|
|
1743
|
-
|
|
1744
|
-
_bend(q, f, l, fwd) { return q.multiply(tq().setFromEuler(_E.set(fwd ? f : -f, fwd ? l : -l, 0, 'XYZ'))); }
|
|
1745
|
-
|
|
1746
|
-
// bone writers: q relative to bind (world = q * bind), or a frame (dirY, latX) directly
|
|
1747
|
-
setBoneQ(name, head, q, scale) {
|
|
1748
|
-
const b = this.bone[name];
|
|
1749
|
-
if (!b) return;
|
|
1750
|
-
b.matrixWorld.compose(head, this._q.copy(q).multiply(this.bindQ[name]), scale || ONE);
|
|
1751
|
-
}
|
|
1752
|
-
setBoneF(name, head, dirY, latX) {
|
|
1753
|
-
const b = this.bone[name];
|
|
1754
|
-
if (!b) return;
|
|
1755
|
-
writeFrame(b.matrixWorld, head, dirY, latX);
|
|
1756
|
-
}
|
|
1757
|
-
|
|
1758
|
-
// ----------------------------------------------------------------- legs
|
|
1759
|
-
// Gait foot target blended with the posture's leg modes (fold, sit, side, tuck, reach).
|
|
1760
|
-
footTarget(leg) {
|
|
1761
|
-
const L = leg.def, g = this.gait, ft = leg.ft, fr = this.fr, cfg = this.cfg, s = cfg.k;
|
|
1762
|
-
const P = this.P, pl = P.legs[L.idx];
|
|
1763
|
-
const c = this.contactPoint(leg, ft.c);
|
|
1764
|
-
leg.contact.copy(c);
|
|
1765
|
-
const n = ft.n;
|
|
1766
|
-
if (leg.state === 'stance') n.copy(leg.plantN);
|
|
1767
|
-
else n.copy(leg.liftN).lerp(this.terrainN(leg.land.x, leg.land.z, tv()), smooth(0, 1, leg.w)).normalize();
|
|
1768
|
-
// paw frame: along the leg's girdle (the chest / pelvis, which a turn bends out of the heading:
|
|
1769
|
-
// along the heading the paws landed up to 15 deg off their girdle; the locomotion's bend only,
|
|
1770
|
-
// girdleFrame)
|
|
1771
|
-
const bodyF = tv(0, 0, 1).applyQuaternion(this.girdleFrame(L));
|
|
1772
|
-
ft.fwd.copy(bodyF).addScaledVector(n, -bodyF.dot(n)).normalize().applyAxisAngle(n, L.s * (L.foot.toeOut ?? 0.06));
|
|
1773
|
-
// a paw set down in a turn is set down turned into it by half the turn its stance will see, so the
|
|
1774
|
-
// leg twists above it by half as much either way (it twisted through the whole stance's turn, up
|
|
1775
|
-
// to 45 deg at a walking turn and 75 deg in a standing pivot)
|
|
1776
|
-
if ((leg.state === 'swing' || !leg.fwdInit) && leg.mode === 'gait') {
|
|
1777
|
-
const pre = clamp(0.5 * this.yawRate * g.DL[L.idx] / Math.max(g.f, 1e-3), -0.4, 0.4);
|
|
1778
|
-
if (Math.abs(pre) > 1e-5) ft.fwd.applyAxisAngle(n, pre);
|
|
1779
|
-
}
|
|
1780
|
-
// a planted paw keeps the heading it landed with (the leg twists above it when the body turns;
|
|
1781
|
-
// a paw yawing about its contact point would scrub its toes and heel over the ground); it turns
|
|
1782
|
-
// back to the body's heading early in the swing
|
|
1783
|
-
if (leg.state === 'stance') {
|
|
1784
|
-
if (leg.fwdInit) ft.fwd.copy(leg.plantFwd).addScaledVector(n, -leg.plantFwd.dot(n)).normalize();
|
|
1785
|
-
else { leg.plantFwd.copy(ft.fwd); leg.fwdInit = true; }
|
|
1786
|
-
} else if (leg.fwdInit) {
|
|
1787
|
-
const k = smooth(0, 0.5, leg.w);
|
|
1788
|
-
if (k < 1) ft.fwd.lerp(tv().copy(leg.plantFwd).addScaledVector(n, -leg.plantFwd.dot(n)), 1 - k).normalize();
|
|
1789
|
-
}
|
|
1790
|
-
// heel lift / toe curl (radians, + raises the heel about the toe tip)
|
|
1791
|
-
const foot = L.foot;
|
|
1792
|
-
const gaitMode = leg.mode === 'gait' ? 1 : 0;
|
|
1793
|
-
let beta = 0;
|
|
1794
|
-
if (leg.state === 'stance') {
|
|
1795
|
-
beta = g.heel * DEG * smooth(0.55, 1.0, leg.u) * gaitMode;
|
|
1796
|
-
// the heel rolls up no faster than the leg pivots over the paw (~ v / leg length): in a very
|
|
1797
|
-
// short stance (sprint, tight banked turn) the paw is still rolling as it lifts off
|
|
1798
|
-
const rate = Math.max(12, 2.2 * this.speed / L.len) * this.frameDt;
|
|
1799
|
-
if (leg.stanceTime > 0 && beta > leg.betaHeel + rate) beta = leg.betaHeel + rate;
|
|
1800
|
-
leg.betaHeel = beta;
|
|
1801
|
-
if (foot.sink) beta -= foot.sink * DEG * Math.sin(Math.PI * clamp(leg.u / 0.8, 0, 1)) * smooth(0.05, 1, this.speed / cfg.sq) * gaitMode;
|
|
1802
|
-
if (foot.heelStrike) beta -= foot.heelStrike * DEG * (1 - smooth(0, 0.15, leg.u)) * gaitMode;
|
|
1803
|
-
} else {
|
|
1804
|
-
const w = leg.w;
|
|
1805
|
-
// a swing shorter than the gait's nominal one (a quick re-step after a landing, a forced
|
|
1806
|
-
// minimum swing) folds the paw less, so it does not fold faster
|
|
1807
|
-
const nom = (1 - g.DL[L.idx]) / Math.max(g.f, 1e-3);
|
|
1808
|
-
leg.swingAmp = gaitMode ? clamp(leg.swingNom / nom, 0.35, 1) : 1;
|
|
1809
|
-
// (the toe curl / hoof flip is half at the walk: a walking horse's hoof barely flips, and the
|
|
1810
|
-
// full flip, carried into the cannon, folded the knee 100 deg in a tenth of a second)
|
|
1811
|
-
const walkC = lerp(0.5, 1, smooth(0.35, 0.6, 1 - g.D));
|
|
1812
|
-
beta = leg.betaStart * (1 - smooth(0.0, 0.45, w)) + foot.curl * DEG * walkC * Math.sin(Math.PI * w) * smooth(0.05, 0.3, w) * (1 - smooth(0.75, 1, w)) * (gaitMode ? 1 : 0.4) * leg.swingAmp;
|
|
1813
|
-
// (gait steps only, like the stance side: an idle re-step landed toe-up and snapped flat)
|
|
1814
|
-
if (foot.heelStrike) beta -= foot.heelStrike * DEG * smooth(0.7, 1, w) * gaitMode;
|
|
1815
|
-
}
|
|
1816
|
-
ft.beta = beta;
|
|
1817
|
-
// front: metacarpal angle from the normal (stance: tilted back, swing: carpal fold)
|
|
1818
|
-
if (L.front) {
|
|
1819
|
-
if (leg.state === 'stance') ft.k = -(L.a0 + foot.flex * DEG * Math.sin(Math.PI * leg.u) * (gaitMode ? 1 : 0.3));
|
|
1820
|
-
else {
|
|
1821
|
-
// swing: from the stance's last angle (an early lift-off leaves the carpus flexed) into the
|
|
1822
|
-
// fold and back to the landing angle; C1 at both ends (no angular-velocity step at lift-off /
|
|
1823
|
-
// touchdown)
|
|
1824
|
-
// (hooves: the knee folds through the first half of the swing and opens over the second, into
|
|
1825
|
-
// the carpal lock above)
|
|
1826
|
-
const hoofS = L.foot.type === 'unguligrade' && cfg.carpusLock > 0;
|
|
1827
|
-
const w = leg.w, fold = hoofS ? smooth(0, 0.4, w) * (1 - smooth(0.42, 0.95, w)) : smooth(0, 0.45, w) * (1 - smooth(0.55, 1, w));
|
|
1828
|
-
ft.k = lerp(leg.kStart, -L.a0, smooth(0, 0.35, w)) + (g.fold * (gaitMode ? 1 : 0.45) + L.a0) * fold * leg.swingAmp;
|
|
1829
|
-
}
|
|
1830
|
-
}
|
|
1831
|
-
ft.dirW = 0;
|
|
1832
|
-
ft.free = 0;
|
|
1833
|
-
if (L.plant && !L.front) {
|
|
1834
|
-
// plantigrade: the metatarsus lies along the foot in stance
|
|
1835
|
-
// (rolled with the heel lift / toe-up like the digits: a metatarsus held flat while the heel
|
|
1836
|
-
// rises pulled the whole foot off the ground at push-off)
|
|
1837
|
-
// (feet.hind.swingFlat 0..1: share of that direction kept through the swing: a Z-folded frog leg,
|
|
1838
|
-
// whose pantograph (metatarsus parallel to the femur) would fold the knee up over the hip)
|
|
1839
|
-
ft.dirW = leg.state === 'stance' ? 1 : 1 - Math.sin(Math.PI * leg.w) * (1 - (foot.swingFlat || 0));
|
|
1840
|
-
// (at the bind pose's metatarsal pitch: the hock sits above the heel pad)
|
|
1841
|
-
ft.dir.copy(ft.fwd).multiplyScalar(Math.cos(L.mtTilt)).addScaledVector(n, -Math.sin(L.mtTilt)).applyAxisAngle(tv().crossVectors(n, ft.fwd).normalize(), beta);
|
|
1842
|
-
} else if (foot.plantRest && !L.front) {
|
|
1843
|
-
// feet.hind.plantRest (rabbit): flat on the ground at rest and in postures (the heel stays
|
|
1844
|
-
// down whatever the body does), on the toes once the animal moves off
|
|
1845
|
-
// (fading out over a swing and back in before touchdown, so steps start and end smoothly)
|
|
1846
|
-
const rest = (1 - smooth(0.03, 0.35, this.speed / cfg.sq) * gaitMode * this.P.legGait) * (leg.state === 'stance' ? 1 : 1 - Math.sin(Math.PI * leg.w));
|
|
1847
|
-
if (rest > 1e-3) {
|
|
1848
|
-
ft.dirW = rest;
|
|
1849
|
-
ft.dir.copy(ft.fwd).multiplyScalar(Math.cos(L.mtTilt)).addScaledVector(n, -Math.sin(L.mtTilt));
|
|
1850
|
-
}
|
|
1851
|
-
}
|
|
1852
|
-
// gait-only values (before the posture blend below), the swing's starting point at lift-off
|
|
1853
|
-
leg.betaGait = ft.beta; leg.kGait = ft.k;
|
|
1854
|
-
// ---- posture leg modes
|
|
1855
|
-
const wSum = pl.fold + pl.sit + pl.side + pl.tuck + pl.reach;
|
|
1856
|
-
if (wSum < 1e-4) return ft;
|
|
1857
|
-
const lx = L.cx * L.s, gy = L.front ? cfg.yS : cfg.yH;
|
|
1858
|
-
const tmpC = tv(), tmpN = tv(), tmpF = tv(), tmpD = tv();
|
|
1859
|
-
const mixIn = (w, beta2, k2, dirW2, wc = w) => {
|
|
1860
|
-
if (w < 1e-4) return;
|
|
1861
|
-
ft.c.lerp(tmpC, wc); ft.n.lerp(tmpN, w).normalize(); ft.fwd.lerp(tmpF, w);
|
|
1862
|
-
ft.fwd.addScaledVector(ft.n, -ft.fwd.dot(ft.n)).normalize();
|
|
1863
|
-
ft.beta = lerp(ft.beta, beta2, w); ft.k = lerp(ft.k, k2, w);
|
|
1864
|
-
if (dirW2 > 0) { ft.dir.lerp(tmpD, w / Math.max(1e-4, ft.dirW + w)).normalize(); }
|
|
1865
|
-
ft.dirW = lerp(ft.dirW, dirW2, w);
|
|
1866
|
-
};
|
|
1867
|
-
const bw = (lxx, ly, lz, out) => out.set(lxx, ly, lz).applyQuaternion(fr.qB).add(fr.origin);
|
|
1868
|
-
// ground placements use the heading frame (a pitched / rolled body must not swing them around)
|
|
1869
|
-
const hw = (lxx, lz, out) => this.bodyToWorld(lxx, 0, lz + P.fwd * s, this.heading, this.pos, out);
|
|
1870
|
-
const onGround = (p) => { p.y = this.terrainH(p.x, p.z); return p; };
|
|
1871
|
-
const hN = (p, out) => this.terrainN(p.x, p.z, out);
|
|
1872
|
-
const fwdFlat = (out) => out.copy(fr.F);
|
|
1873
|
-
if (pl.fold > 0) {
|
|
1874
|
-
// folded under the body (lying sternal): forelegs forward along the ground (sphinx),
|
|
1875
|
-
// hind legs folded with the metatarsus flat and the paw beside the elbow
|
|
1876
|
-
// (actions.fold.front = { z, k, beta }: folded under the chest instead (hoofed animals kneel):
|
|
1877
|
-
// hoof at zS + z * leg length, metacarpal angle k (rad), hoof flip beta (rad))
|
|
1878
|
-
const fo = L.front ? cfg.actions.fold?.front : null;
|
|
1879
|
-
if (L.front) onGround(hw(lx * 1.05, fo ? cfg.zS + fo.z * L.len : cfg.zS + 0.5 * L.Lr + L.Lmc, tmpC));
|
|
1880
|
-
else onGround(hw(lx * 1.7 + L.s * 0.04 * s, cfg.zH + L.L1 * 0.75 + L.L3 * 0.5, tmpC));
|
|
1881
|
-
hN(tmpC, tmpN); fwdFlat(tmpF);
|
|
1882
|
-
tmpD.copy(tmpF);
|
|
1883
|
-
mixIn(pl.fold, fo ? fo.beta || 0 : 0, L.front ? (fo ? fo.k : -1.45) : 0, L.front ? 0 : 1);
|
|
1884
|
-
}
|
|
1885
|
-
if (pl.sit > 0) {
|
|
1886
|
-
// sitting: forelegs straight under the chest, hind feet flat beside the haunches
|
|
1887
|
-
if (L.front) {
|
|
1888
|
-
// under the (raised, pitched-back) shoulder
|
|
1889
|
-
const shz = (fr.shC.x - this.pos.x) * fr.F.x + (fr.shC.z - this.pos.z) * fr.F.z - P.fwd * s;
|
|
1890
|
-
onGround(hw(lx, shz + 0.02 * L.len, tmpC));
|
|
1891
|
-
} else onGround(hw(lx * 1.35, cfg.zH + L.L1 * 0.7 + L.L3 * 0.4, tmpC));
|
|
1892
|
-
hN(tmpC, tmpN); fwdFlat(tmpF); tmpD.copy(tmpF);
|
|
1893
|
-
// (hind: the paw stays where it stood while the haunches go down and the hock folds over it,
|
|
1894
|
-
// then steps forward to beside the haunch in a short lifted step; it slid forward along the
|
|
1895
|
-
// ground through the whole sit, the legs straightening out ahead of the lowering rump)
|
|
1896
|
-
let wc = pl.sit;
|
|
1897
|
-
if (!L.front) { wc = smooth(0.4, 0.95, pl.sit); tmpC.y += Math.sin(Math.PI * wc) * 0.1 * L.len; }
|
|
1898
|
-
// (hooves: the hind hoof flips back so the cannon and the hock rest on the ground; standing on its
|
|
1899
|
-
// toe it held the hock and the cannon in the air and the rump off the ground)
|
|
1900
|
-
const flipH = !L.front && L.foot.type === 'unguligrade' && L.C ? -48 * DEG : 0;
|
|
1901
|
-
mixIn(pl.sit, flipH, L.front ? -L.a0 * 0.6 : 0, L.front ? 0 : 1, wc);
|
|
1902
|
-
}
|
|
1903
|
-
if (pl.side > 0) {
|
|
1904
|
-
// lying on the side: legs out from the body, resting on the ground
|
|
1905
|
-
const out = pl.spread;
|
|
1906
|
-
const lz = (L.front ? cfg.zS + 0.18 * L.len : cfg.zH - 0.02 * L.len) + (L.s * Math.sign(P.roll || 1) > 0 ? 0.12 : 0) * L.len * out;
|
|
1907
|
-
bw(lx * 1.2, gy - L.len * lerp(0.85, 0.95, out), lz, tmpC);
|
|
1908
|
-
const gh = this.terrainH(tmpC.x, tmpC.z) + 0.03 * s;
|
|
1909
|
-
tmpC.y = Math.max(gh, lerp(tmpC.y, gh, 0.85));
|
|
1910
|
-
tmpN.set(0, 1, 0).applyQuaternion(fr.qB); tmpF.set(0, 0, 1).applyQuaternion(fr.qB);
|
|
1911
|
-
tmpD.copy(tmpF);
|
|
1912
|
-
mixIn(pl.side, 25 * DEG, L.front ? -0.35 : 0, 0);
|
|
1913
|
-
}
|
|
1914
|
-
if (pl.tuck > 0) {
|
|
1915
|
-
// flight: extended (tuck < 0.5 of the curve) or tucked under the body, relative to the body
|
|
1916
|
-
const e = pl.extend;
|
|
1917
|
-
// (hooves: the long pastern + hoof below the fetlock counts too: the leg reaches its full length)
|
|
1918
|
-
const hoof = L.foot.type === 'unguligrade', Lt = hoof ? L.len + L.M.y : L.len;
|
|
1919
|
-
if (L.front) bw(lx, gy - Lt * lerp(0.62, 0.88, e), cfg.zS + L.len * lerp(-0.12, 0.45, e), tmpC);
|
|
1920
|
-
else bw(lx, gy - Lt * lerp(0.6, 0.8, e), cfg.zH + L.len * lerp(0.25, -0.45, e), tmpC);
|
|
1921
|
-
tmpN.set(0, 1, 0).applyQuaternion(fr.qB); tmpF.set(0, 0, 1).applyQuaternion(fr.qB);
|
|
1922
|
-
const gh = this.terrainH(tmpC.x, tmpC.z);
|
|
1923
|
-
if (tmpC.y < gh + 0.02 * s) tmpC.y = gh + 0.02 * s;
|
|
1924
|
-
// (hooves reach out with the cannon and pastern in line: a straight foreleg to land on)
|
|
1925
|
-
mixIn(pl.tuck, lerp(L.front ? 60 : 50, hoof ? 0 : 10, e) * DEG, L.front ? lerp(g.fold || 1.8, hoof ? -L.a0 : 0.1, e) : 0, 0);
|
|
1926
|
-
}
|
|
1927
|
-
if (pl.reach > 0) {
|
|
1928
|
-
// reach for a point (strike, pounce, kick)
|
|
1929
|
-
tmpC.copy(pl.target);
|
|
1930
|
-
tmpN.set(0, 1, 0).applyQuaternion(fr.qB);
|
|
1931
|
-
tmpF.set(0, 0, 1).applyQuaternion(fr.qB);
|
|
1932
|
-
const gh = this.terrainH(tmpC.x, tmpC.z);
|
|
1933
|
-
if (tmpC.y < gh) tmpC.y = gh;
|
|
1934
|
-
mixIn(pl.reach, L.front ? 10 * DEG : 35 * DEG, L.front ? 0.35 : 0, 0);
|
|
1935
|
-
ft.free = pl.reach;
|
|
1936
|
-
}
|
|
1937
|
-
// while a foot moves between the gait and a ground pose it steps (small arc) instead of sliding
|
|
1938
|
-
// (measured from the gait's own contact point, continuous through a touchdown: leg.plant jumps to the
|
|
1939
|
-
// landing point when a swing ends under a posture blend, and the arc jumped with it)
|
|
1940
|
-
const gw = pl.fold + pl.sit + pl.side;
|
|
1941
|
-
if (gw > 0 && gw < 1) ft.c.y += 4 * gw * (1 - gw) * 0.05 * s * Math.min(1, ft.c.distanceTo(leg.contact) / (0.08 * s));
|
|
1942
|
-
return ft;
|
|
1943
|
-
}
|
|
1944
|
-
|
|
1945
|
-
solveLeg(leg, dt) {
|
|
1946
|
-
const L = leg.def, S = L.S, J = this.J, fr = this.fr, cfg = this.cfg;
|
|
1947
|
-
const ft = this.footTarget(leg);
|
|
1948
|
-
leg.betaMixed = ft.beta;
|
|
1949
|
-
const c = ft.c, n = ft.n, fwd = ft.fwd;
|
|
1950
|
-
const latA = tv().crossVectors(n, fwd).normalize();
|
|
1951
|
-
// foot points in the contact frame
|
|
1952
|
-
const Mflat = footPoint(tv(), c, latA, n, fwd, L.M), T = footPoint(tv(), c, latA, n, fwd, L.T);
|
|
1953
|
-
let M, C = null;
|
|
1954
|
-
if (ft.beta >= 0 || L.C) {
|
|
1955
|
-
// heel lift / hoof breakover (beta >= 0): the digit rolls about the toe; fetlock sink (beta < 0,
|
|
1956
|
-
// hooves): the pastern turns about the coffin joint and the hoof stays flat on the ground
|
|
1957
|
-
const Cf = L.C ? footPoint(tv(), c, latA, n, fwd, L.C) : null;
|
|
1958
|
-
M = tv(); C = Cf ? tv() : null;
|
|
1959
|
-
placeDigit(ft.beta, Mflat, Cf, T, latA, M, C);
|
|
1960
|
-
// toe-off on demand: a planted foot that the leg can no longer reach rolls up about the toe
|
|
1961
|
-
// (heel rises, a sinking fetlock comes back up first) just enough, so the toe stays where it
|
|
1962
|
-
// was planted
|
|
1963
|
-
this.toeOff(leg, ft, Mflat, Cf, T, M, latA);
|
|
1964
|
-
// low-passed so the toe-off does not pass the body's bounce straight into the wrist
|
|
1965
|
-
// (the fetlock's return faster, at 140 rad/s: at 70 it came back 29 % in a frame, and a galloping
|
|
1966
|
-
// horse's 2-frame fore stance in a banked turn dragged its hoof 1.1-1.7 % S while it sank)
|
|
1967
|
-
const add = Math.max(0, leg.toeS.step(leg.betaAdd, 30, dt)) + (L.C ? Math.max(0, leg.sinkS.step(leg.sinkLift, 140, dt)) : 0);
|
|
1968
|
-
if (add > 1e-5) {
|
|
1969
|
-
ft.beta += add;
|
|
1970
|
-
placeDigit(ft.beta, Mflat, Cf, T, latA, M, C);
|
|
1971
|
-
// (a plantigrade metatarsus laid along the foot rolls up with it)
|
|
1972
|
-
if (L.plant && !L.front && ft.dirW > 0) ft.dir.applyAxisAngle(latA, add);
|
|
1973
|
-
}
|
|
1974
|
-
} else {
|
|
1975
|
-
// toe-up (heel strike) / fetlock sink: pivot the foot about its back point instead
|
|
1976
|
-
const qBeta = tqa(latA, ft.beta);
|
|
1977
|
-
M = Mflat;
|
|
1978
|
-
if (L.plant && !L.front) { const Hf = footPoint(tv(), c, latA, n, fwd, L.H); T.sub(Hf).applyQuaternion(qBeta).add(Hf); M.sub(Hf).applyQuaternion(qBeta).add(Hf); }
|
|
1979
|
-
else if (L.foot.type === 'unguligrade') M = tv().copy(Mflat).sub(T).applyQuaternion(qBeta).add(T);
|
|
1980
|
-
else T.sub(M).applyQuaternion(qBeta).add(M);
|
|
1981
|
-
}
|
|
1982
|
-
leg.betaLast = ft.beta;
|
|
1983
|
-
leg.betaGaitLast = leg.betaGait + (ft.beta - (leg.betaMixed ?? ft.beta)); // gait heel angle + toe-off
|
|
1984
|
-
// swing: the coffin joint flexes too (the sole turns up and back)
|
|
1985
|
-
// (gait swings only: faded out under a posture's leg modes, so a swing that a landing plants
|
|
1986
|
-
// mid-flight does not drop its flexion in one frame)
|
|
1987
|
-
if (C && leg.state === 'swing' && L.foot.hoofFlex) {
|
|
1988
|
-
const pl = this.P.legs[L.idx], wp = Math.min(1, pl.fold + pl.sit + pl.side + pl.tuck + pl.reach);
|
|
1989
|
-
const hf = L.foot.hoofFlex * DEG * Math.sin(Math.PI * leg.w) * (leg.mode === 'gait' ? 1 : 0.4) * (1 - wp);
|
|
1990
|
-
if (hf > 1e-5) T.sub(C).applyAxisAngle(latA, hf).add(C);
|
|
1991
|
-
}
|
|
1992
|
-
leg.C = C;
|
|
1993
|
-
// (the knee / elbow bend plane follows the leg's own girdle, which a turn bends: the locomotion's
|
|
1994
|
-
// bend only, girdleFrame; a sleeping curl's bend of the chest turned a folded foreleg's elbow out
|
|
1995
|
-
// and its forearm into the ground)
|
|
1996
|
-
const qG = this.girdleFrame(L);
|
|
1997
|
-
const bodyF = tv(0, 0, 1).applyQuaternion(qG), bodyX = tv(1, 0, 0).applyQuaternion(qG);
|
|
1998
|
-
const sprawl = cfg.stance.sprawl;
|
|
1999
|
-
const dbg = this.debug.targets;
|
|
2000
|
-
if (!L.front) this.solveHind(leg, dt, ft, M, T, latA, bodyF, bodyX);
|
|
2001
|
-
else this.solveFront(leg, dt, ft, M, T, latA, bodyF, bodyX);
|
|
2002
|
-
}
|
|
2003
|
-
|
|
2004
|
-
// hind: pantograph IK from hip to the MTP, then 2-bone hip -> hock
|
|
2005
|
-
solveHind(leg, dt, ft, M, T, latA, bodyF, bodyX) {
|
|
2006
|
-
const L = leg.def, J = this.J, fr = this.fr, cfg = this.cfg;
|
|
2007
|
-
const c = ft.c, n = ft.n, fwd = ft.fwd;
|
|
2008
|
-
const sprawl = cfg.stance.sprawl;
|
|
2009
|
-
const dbg = this.debug.targets;
|
|
2010
|
-
// ---------- hind: pantograph IK from hip to the MTP, then 2-bone hip -> hock
|
|
2011
|
-
const hip = tv().copy(L.jHip).sub(J.lumbosacral).applyQuaternion(fr.qPel).add(fr.lumbo);
|
|
2012
|
-
const pole = tv().copy(bodyF).addScaledVector(bodyX, L.s * (0.12 + sprawl * 1.6));
|
|
2013
|
-
const d = tv().subVectors(M, hip);
|
|
2014
|
-
let dist = d.length();
|
|
2015
|
-
// (feet.hind.zfold: a Z-folded limb whose metatarsus is laid along the foot (frog: plantigrade tarsus)
|
|
2016
|
-
// folds far tighter than the pantograph's femur-parallel metatarsus allows: the pantograph's minimum
|
|
2017
|
-
// reach fades out with the imposed metatarsus direction; the hip -> hock solve below still limits it)
|
|
2018
|
-
const maxD = (L.Av + L.L2) * 0.9995, minD = Math.abs(L.Av - L.L2) * (L.foot.zfold ? 1 - ft.dirW : 1) + 0.01 * cfg.k;
|
|
2019
|
-
const ovM = dist / maxD; // (overreach of the pantograph stage; the hip -> hock stage below may be the limit)
|
|
2020
|
-
const lp = this.P.legs[L.idx];
|
|
2021
|
-
const locked = cfg.stanceSoftMargin !== null && leg.state === 'stance' && this.P.legGait > 0.5 && lp.fold + lp.sit + lp.side + lp.tuck + lp.reach < 0.005;
|
|
2022
|
-
const dSoft = (L.Av + L.L2) * (1 - (locked ? cfg.stanceSoftMargin : 0.035));
|
|
2023
|
-
if (dist > dSoft) dist = dSoft + (maxD - dSoft) * (1 - Math.exp(-(dist - dSoft) / (maxD - dSoft)));
|
|
2024
|
-
dist = Math.max(dist, minD);
|
|
2025
|
-
const e1 = d.normalize();
|
|
2026
|
-
const e2 = this.bendDir(leg, 0, pole, e1);
|
|
2027
|
-
const nrm = tv().crossVectors(e1, e2).normalize();
|
|
2028
|
-
const cosA = clamp((L.Av * L.Av + dist * dist - L.L2 * L.L2) / (2 * L.Av * dist), -1, 1);
|
|
2029
|
-
const sinA = Math.sqrt(1 - cosA * cosA);
|
|
2030
|
-
const u = tv().copy(e1).multiplyScalar(cosA).addScaledVector(e2, sinA).applyAxisAngle(nrm, -L.gamma);
|
|
2031
|
-
const mt = tv().copy(u).applyAxisAngle(nrm, L.delta);
|
|
2032
|
-
const Mreach = tv().copy(hip).addScaledVector(e1, dist);
|
|
2033
|
-
// posture / plantigrade: blend the metatarsus toward a prescribed direction (hock -> MTP)
|
|
2034
|
-
if (ft.dirW > 1e-4) {
|
|
2035
|
-
mt.lerp(ft.dir, ft.dirW).normalize();
|
|
2036
|
-
// a metatarsus laid along the ground keeps the hock at its bind height over the terrain under it
|
|
2037
|
-
// (bumps under the heel, a body pitched nose-up), so the heel does not sink into the ground
|
|
2038
|
-
// (or at its rest height, feet.hind.restTilt, when the bind pose stands with the heel lifted)
|
|
2039
|
-
const hx = Mreach.x - mt.x * L.L3, hz = Mreach.z - mt.z * L.L3;
|
|
2040
|
-
const floor = this.terrainH(hx, hz) + Math.min(L.H.y, L.M.y + L.L3 * Math.sin(L.mtTilt)) * 0.9;
|
|
2041
|
-
const hy = Mreach.y - mt.y * L.L3;
|
|
2042
|
-
if (hy < floor && (L.plant || L.foot.plantRest)) {
|
|
2043
|
-
const my = clamp((Mreach.y - floor) / L.L3, -0.99, 0.99), hl = Math.hypot(mt.x, mt.z) || 1, r = Math.sqrt(1 - my * my) / hl;
|
|
2044
|
-
mt.set(mt.x * r, my, mt.z * r);
|
|
2045
|
-
}
|
|
2046
|
-
}
|
|
2047
|
-
const hock = tv().copy(Mreach).addScaledVector(mt, -L.L3);
|
|
2048
|
-
// 2-bone hip -> hock
|
|
2049
|
-
const dh = tv().subVectors(hock, hip);
|
|
2050
|
-
let dd = dh.length();
|
|
2051
|
-
const max2 = (L.L1 + L.L2) * 0.999, soft2 = (L.L1 + L.L2) * (1 - (locked ? cfg.stanceSoftMargin : 0.03)), min2 = Math.abs(L.L1 - L.L2) + 0.01 * cfg.k;
|
|
2052
|
-
// overreach for the predictive lift-off: the larger of the two stages. A metatarsus laid along
|
|
2053
|
-
// the foot (plantigrade: bear, rat) puts the hock where the foot says, and the hip -> hock stage
|
|
2054
|
-
// runs out first: the pantograph read 0.9 while the planted toe was dragged 3 cm up and forward
|
|
2055
|
-
this.setOverreach(leg, Math.max(ovM, dd / max2), dt);
|
|
2056
|
-
leg.ikJ.copy(hip); leg.ikT1.copy(M); leg.ikM1 = maxD; leg.ikT2.copy(hock); leg.ikM2 = max2;
|
|
2057
|
-
if (dd > soft2) dd = soft2 + (max2 - soft2) * (1 - Math.exp(-(dd - soft2) / (max2 - soft2)));
|
|
2058
|
-
dd = Math.max(dd, min2);
|
|
2059
|
-
const f1 = dh.normalize();
|
|
2060
|
-
const f2 = this.bendDir(leg, 1, pole, f1);
|
|
2061
|
-
const cB = clamp((L.L1 * L.L1 + dd * dd - L.L2 * L.L2) / (2 * L.L1 * dd), -1, 1);
|
|
2062
|
-
this.liftOffGround(hip, f1, f2, cB * L.L1, Math.sqrt(1 - cB * cB) * L.L1, leg, tv().copy(bodyX).multiplyScalar(L.s));
|
|
2063
|
-
const knee = tv().copy(hip).addScaledVector(tv().copy(f1).multiplyScalar(cB).addScaledVector(f2, Math.sqrt(1 - cB * cB)), L.L1);
|
|
2064
|
-
const hockR = tv().copy(hip).addScaledVector(f1, dd);
|
|
2065
|
-
const off = tv().subVectors(hockR, hock);
|
|
2066
|
-
const Mr = tv().copy(Mreach).add(off);
|
|
2067
|
-
const Tr = tv().copy(T).add(Mreach).sub(M).add(off);
|
|
2068
|
-
const lat = tv().crossVectors(f1, f2).negate();
|
|
2069
|
-
if (lat.lengthSq() < 1e-8) lat.copy(bodyX);
|
|
2070
|
-
this.setBoneF(L.n.femur, hip, tv().subVectors(knee, hip), lat);
|
|
2071
|
-
this.setBoneF(L.n.tibia, knee, tv().subVectors(hockR, knee), lat);
|
|
2072
|
-
this.setBoneF(L.n.metatarsus, hockR, tv().subVectors(Mr, hockR), lat);
|
|
2073
|
-
let Cr = null;
|
|
2074
|
-
if (leg.C) {
|
|
2075
|
-
Cr = tv().copy(leg.C).add(Mreach).sub(M).add(off);
|
|
2076
|
-
this.setBoneF(L.n.hpaw, Mr, tv().subVectors(Cr, Mr), latA);
|
|
2077
|
-
this.setBoneF(L.hoof, Cr, tv().subVectors(Tr, Cr), latA);
|
|
2078
|
-
} else this.setBoneF(L.n.hpaw, Mr, tv().subVectors(Tr, Mr), latA);
|
|
2079
|
-
this.storeLeg(leg, hip, knee, hockR, Mr, Tr, null, Cr);
|
|
2080
|
-
dbg[L.idx * 2].copy(c); dbg[L.idx * 2 + 1].copy(M);
|
|
2081
|
-
}
|
|
2082
|
-
|
|
2083
|
-
// front: scapula rotation + 2-bone IK to a wrist target
|
|
2084
|
-
solveFront(leg, dt, ft, M, T, latA, bodyF, bodyX) {
|
|
2085
|
-
const L = leg.def, J = this.J, fr = this.fr, cfg = this.cfg;
|
|
2086
|
-
const c = ft.c, n = ft.n, fwd = ft.fwd;
|
|
2087
|
-
const sprawl = cfg.stance.sprawl;
|
|
2088
|
-
const dbg = this.debug.targets;
|
|
2089
|
-
// ---------- front: scapula rotation + 2-bone IK to a wrist target
|
|
2090
|
-
const qCh = fr.qCh;
|
|
2091
|
-
const vn = this.speed / cfg.sq;
|
|
2092
|
-
let prot = 0;
|
|
2093
|
-
if (leg.state === 'stance') prot = lerp(0.18, -0.25, leg.u);
|
|
2094
|
-
else prot = lerp(leg.protStart, 0.18, smooth(0.1, 0.9, leg.w));
|
|
2095
|
-
prot *= smooth(0.05, 1.0, vn) * lerp(0.75, 1, this.gait.gallop) * this.P.legGait;
|
|
2096
|
-
const pivotW = tv().copy(L.pivot).sub(J.chestMid).applyQuaternion(qCh).add(fr.chestMid);
|
|
2097
|
-
const dir = tv().copy(n).multiplyScalar(Math.cos(ft.k)).addScaledVector(fwd, Math.sin(ft.k));
|
|
2098
|
-
dir.applyAxisAngle(latA, ft.beta * L.foot.couple);
|
|
2099
|
-
// out of reach: straighten the carpus / pastern toward the shoulder just enough (closed form)
|
|
2100
|
-
// before anything else gives, so a planted paw stays planted
|
|
2101
|
-
{
|
|
2102
|
-
const sh0 = tv().copy(L.jShoulder).sub(L.pivot).applyQuaternion(qCh).add(pivotW);
|
|
2103
|
-
const u = tv().subVectors(sh0, M);
|
|
2104
|
-
const D = u.length();
|
|
2105
|
-
const dS = (L.Lh + L.Lr) * 0.95;
|
|
2106
|
-
if (D > 1e-6) {
|
|
2107
|
-
u.multiplyScalar(1 / D);
|
|
2108
|
-
const c0 = clamp(dir.dot(u), -1, 1);
|
|
2109
|
-
const cT = clamp((D * D + L.Lmc * L.Lmc - dS * dS) / (2 * D * L.Lmc), -1, 1);
|
|
2110
|
-
const th0 = Math.acos(c0), thT = Math.acos(cT);
|
|
2111
|
-
// softplus of the angular deficit: C-infinity onset, so body bob cannot make it chatter
|
|
2112
|
-
const wS = 0.2, x = (th0 - thT) / wS;
|
|
2113
|
-
const stW = leg.state === 'stance' ? 1 : 1 - smooth(0, 0.25, leg.w) + smooth(0.7, 1, leg.w);
|
|
2114
|
-
const rotT = Math.min(th0, wS * (x > 20 ? x : Math.log1p(Math.exp(x)))) * this.P.legGait * stW;
|
|
2115
|
-
const rot = Math.min(th0, Math.max(0, leg.strS.step(rotT, 35, dt)));
|
|
2116
|
-
if (rot > 1e-5) {
|
|
2117
|
-
const ax = tv().crossVectors(dir, u);
|
|
2118
|
-
if (ax.lengthSq() > 1e-10) dir.applyAxisAngle(ax.normalize(), rot);
|
|
2119
|
-
}
|
|
2120
|
-
}
|
|
2121
|
-
}
|
|
2122
|
-
// lying / sitting / folding (ground poses): the wrist stays its skin's thickness above the ground
|
|
2123
|
-
// (L.rW), and, folding under the chest, below the shoulder: half way through a cow's stand-up the
|
|
2124
|
-
// blend of the kneeling and the standing cannon lifted the wrist 4 cm over the lowered shoulder,
|
|
2125
|
-
// the arm folded flat with the elbow up and the cannon turned over (160 rad/s*). Both soft
|
|
2126
|
-
// (softplus onsets, never past the hard limit): the cannon tilts about the fetlock just enough
|
|
2127
|
-
const plG = this.P.legs[L.idx], wG = Math.min(1, 4 * (plG.fold + plG.sit + plG.side)), wF = Math.min(1, 4 * plG.fold);
|
|
2128
|
-
if (wG > 1e-3) {
|
|
2129
|
-
const hz = Math.hypot(dir.x, dir.z);
|
|
2130
|
-
const floorW = this.terrainH(M.x + dir.x * L.Lmc, M.z + dir.z * L.Lmc) + L.rW;
|
|
2131
|
-
const sh0 = tv().copy(L.jShoulder).sub(L.pivot).applyQuaternion(qCh).add(pivotW);
|
|
2132
|
-
// (the ceiling never below the floor: a lion curled up asleep, its shoulder low, had the two
|
|
2133
|
-
// cross and its wrist jumped 47 mm when the ceiling let go)
|
|
2134
|
-
const lo = (floorW - M.y) / L.Lmc, hi = Math.max(lo + 0.1, (sh0.y - 0.1 * (L.Lh + L.Lr) - M.y) / L.Lmc);
|
|
2135
|
-
let dy = dir.y;
|
|
2136
|
-
const xl = (lo - dy) / 0.05, xh = (dy - hi) / 0.15;
|
|
2137
|
-
dy += wG * 0.05 * (xl > 20 ? xl : Math.log1p(Math.exp(xl))) - wF * 0.15 * (xh > 20 ? xh : Math.log1p(Math.exp(xh)));
|
|
2138
|
-
dy = clamp(dy, -1, 1);
|
|
2139
|
-
if (hz > 1e-6 && dy !== dir.y) { const k2 = Math.sqrt(Math.max(0, 1 - dy * dy)) / hz; dir.set(dir.x * k2, dy, dir.z * k2); }
|
|
2140
|
-
}
|
|
2141
|
-
const Wt = tv().copy(M).addScaledVector(dir, L.Lmc);
|
|
2142
|
-
// hooves: the carpus locks straight under load (the stay apparatus): through the stance the
|
|
2143
|
-
// forearm and cannon are aimed as one piece at the fetlock, the elbow and shoulder taking up the
|
|
2144
|
-
// body's height; it bent 30-45 deg in a lowered girdle's stance and snapped straight at each step
|
|
2145
|
-
// (it straightens at the end of the swing, so the leg lands as one straight column and the lock
|
|
2146
|
-
// runs on into the stance without a step)
|
|
2147
|
-
// (eased in and out: switched off at once by a jump's take-off or a posture, the cannon flicked)
|
|
2148
|
-
if (L.foot.type === 'unguligrade' && cfg.carpusLock > 0) {
|
|
2149
|
-
const wT = leg.mode === 'gait' && !this.air.active ? (leg.state === 'stance' ? 1 - smooth(0.75, 1, leg.u) : smooth(0.45, 0.97, leg.w)) * smooth(0.5, 1, this.P.legGait) * cfg.carpusLock : 0;
|
|
2150
|
-
leg.lockW = expTo(leg.lockW ?? 0, wT, dt, wT > (leg.lockW ?? 0) ? 0.02 : 0.06);
|
|
2151
|
-
const wl = leg.lockW;
|
|
2152
|
-
if (wl > 1e-3) {
|
|
2153
|
-
const shP = tv().copy(L.jShoulder).sub(L.pivot).applyQuaternion(tqc(qCh).multiply(tqa(X1, -prot))).add(pivotW);
|
|
2154
|
-
const a = L.Lh, bL = L.Lr + L.Lmc, e1 = tv().subVectors(M, shP);
|
|
2155
|
-
const d = clamp(e1.length(), Math.abs(a - bL) + 0.01 * cfg.k, (a + bL) * 0.995);
|
|
2156
|
-
e1.normalize();
|
|
2157
|
-
const e2 = tv().copy(bodyF).negate().addScaledVector(e1, bodyF.dot(e1));
|
|
2158
|
-
if (e2.lengthSq() > 1e-8) {
|
|
2159
|
-
e2.normalize();
|
|
2160
|
-
const cA = clamp((a * a + d * d - bL * bL) / (2 * a * d), -1, 1), sA = Math.sqrt(1 - cA * cA);
|
|
2161
|
-
const elbow = tv().copy(shP).addScaledVector(e1, a * cA).addScaledVector(e2, a * sA);
|
|
2162
|
-
const wristL = tv().subVectors(M, elbow).normalize().multiplyScalar(L.Lr).add(elbow);
|
|
2163
|
-
Wt.lerp(wristL, wl);
|
|
2164
|
-
}
|
|
2165
|
-
}
|
|
2166
|
-
}
|
|
2167
|
-
// shoulder position on its arc about the pivot: sh(pr) = pivot + A cos pr + B sin pr + C
|
|
2168
|
-
// (rotation about the chest's lateral axis), so the golden-section search is all scalar math
|
|
2169
|
-
const r0 = tv().copy(L.jShoulder).sub(L.pivot).applyQuaternion(qCh);
|
|
2170
|
-
const ax = tv(1, 0, 0).applyQuaternion(qCh);
|
|
2171
|
-
const Cc = tv().copy(ax).multiplyScalar(ax.dot(r0));
|
|
2172
|
-
const A = tv().copy(r0).sub(Cc), B = tv().crossVectors(ax, A).negate();
|
|
2173
|
-
const wx = Wt.x - pivotW.x - Cc.x, wy = Wt.y - pivotW.y - Cc.y, wz = Wt.z - pivotW.z - Cc.z;
|
|
2174
|
-
const lim = (L.Lh + L.Lr) * 0.955;
|
|
2175
|
-
const prot0 = prot, kS = 0.0004 * cfg.k * cfg.k;
|
|
2176
|
-
const sc = SC;
|
|
2177
|
-
sc[0] = A.x; sc[1] = A.y; sc[2] = A.z; sc[3] = B.x; sc[4] = B.y; sc[5] = B.z;
|
|
2178
|
-
sc[6] = wx; sc[7] = wy; sc[8] = wz; sc[9] = lim; sc[10] = prot0; sc[11] = kS;
|
|
2179
|
-
prot = scapBestFit(sc, this.lod === 0 ? 26 : 16);
|
|
2180
|
-
if (!leg.protInit) { leg.protS.reset(prot); leg.protInit = true; }
|
|
2181
|
-
prot = leg.protS.step(prot, 24, dt);
|
|
2182
|
-
const qSc = tqc(qCh).multiply(tqa(X1, -prot));
|
|
2183
|
-
const shoulder = tv().copy(L.jShoulder).sub(L.pivot).applyQuaternion(qSc).add(pivotW);
|
|
2184
|
-
const scapTop = tv().copy(L.jScapTop).sub(L.pivot).applyQuaternion(qSc).add(pivotW);
|
|
2185
|
-
// elbow pole: backward; feet.front.elbowDown tilts it down (a paw reached straight forward to the
|
|
2186
|
-
// face keeps its elbow below instead of flipping through the pole: rabbit grooming, boxing)
|
|
2187
|
-
const pole = tv().copy(bodyF).negate().addScaledVector(bodyX, L.s * (0.08 + sprawl * 1.6));
|
|
2188
|
-
if (L.foot.elbowDown) pole.addScaledVector(tv(0, 1, 0).applyQuaternion(fr.qB), -L.foot.elbowDown);
|
|
2189
|
-
const d = tv().subVectors(Wt, shoulder);
|
|
2190
|
-
let dist = d.length();
|
|
2191
|
-
const maxD = (L.Lh + L.Lr) * 0.998, minD = Math.abs(L.Lh - L.Lr) + 0.02 * cfg.k;
|
|
2192
|
-
this.setOverreach(leg, dist / maxD, dt);
|
|
2193
|
-
leg.ikJ.copy(shoulder); leg.ikT1.copy(Wt); leg.ikM1 = maxD; leg.ikM2 = 0;
|
|
2194
|
-
const lp = this.P.legs[L.idx];
|
|
2195
|
-
const locked = cfg.stanceSoftMargin !== null && leg.state === 'stance' && this.P.legGait > 0.5 && lp.fold + lp.sit + lp.side + lp.tuck + lp.reach < 0.005;
|
|
2196
|
-
const dSoft = (L.Lh + L.Lr) * (1 - (locked ? cfg.stanceSoftMargin : 0.045));
|
|
2197
|
-
if (dist > dSoft) dist = dSoft + (maxD - dSoft) * (1 - Math.exp(-(dist - dSoft) / (maxD - dSoft)));
|
|
2198
|
-
dist = Math.max(dist, minD);
|
|
2199
|
-
const e1 = d.normalize();
|
|
2200
|
-
const e2 = this.bendDir(leg, 0, pole, e1);
|
|
2201
|
-
const nrm = tv().crossVectors(e1, e2).normalize();
|
|
2202
|
-
const cosA = clamp((L.Lh * L.Lh + dist * dist - L.Lr * L.Lr) / (2 * L.Lh * dist), -1, 1);
|
|
2203
|
-
const sinA = Math.sqrt(1 - cosA * cosA);
|
|
2204
|
-
this.liftOffGround(shoulder, e1, e2, cosA * L.Lh, sinA * L.Lh, leg, tv().copy(bodyX).multiplyScalar(L.s));
|
|
2205
|
-
nrm.crossVectors(e1, e2).normalize();
|
|
2206
|
-
const elbow = tv().copy(shoulder).addScaledVector(tv().copy(e1).multiplyScalar(cosA).addScaledVector(e2, sinA), L.Lh);
|
|
2207
|
-
const wrist = tv().copy(shoulder).addScaledVector(e1, dist);
|
|
2208
|
-
const off = tv().subVectors(wrist, Wt);
|
|
2209
|
-
const Mr = tv().copy(M).add(off), Tr = tv().copy(T).add(off);
|
|
2210
|
-
this.setBoneF(L.n.scapula, scapTop, tv().subVectors(shoulder, scapTop), tv(1, 0, 0).applyQuaternion(qSc));
|
|
2211
|
-
this.setBoneF(L.n.humerus, shoulder, tv().subVectors(elbow, shoulder), nrm);
|
|
2212
|
-
this.setBoneF(L.n.radius, elbow, tv().subVectors(wrist, elbow), nrm);
|
|
2213
|
-
// (lateral reference: the arm-plane normal; where it turns parallel to the metacarpus (a forearm
|
|
2214
|
-
// held level with the paw hanging, e.g. a paw raised to the face) the foot's lateral axis is blended
|
|
2215
|
-
// in continuously so the frame never degenerates)
|
|
2216
|
-
const mcD = tv().subVectors(Mr, wrist), mcL = mcD.length();
|
|
2217
|
-
const par = mcL > 1e-9 ? Math.abs(nrm.dot(mcD)) / mcL : 0;
|
|
2218
|
-
this.setBoneF(L.n.metacarpus, wrist, mcD, par > 0.8 ? tv().copy(nrm).addScaledVector(latA, smooth(0.8, 0.97, par)) : nrm);
|
|
2219
|
-
let Cr = null;
|
|
2220
|
-
if (leg.C) {
|
|
2221
|
-
Cr = tv().copy(leg.C).add(off);
|
|
2222
|
-
this.setBoneF(L.n.fpaw, Mr, tv().subVectors(Cr, Mr), latA);
|
|
2223
|
-
this.setBoneF(L.hoof, Cr, tv().subVectors(Tr, Cr), latA);
|
|
2224
|
-
} else this.setBoneF(L.n.fpaw, Mr, tv().subVectors(Tr, Mr), latA);
|
|
2225
|
-
this.storeLeg(leg, shoulder, elbow, wrist, Mr, Tr, scapTop, Cr);
|
|
2226
|
-
dbg[L.idx * 2].copy(c); dbg[L.idx * 2 + 1].copy(Wt);
|
|
2227
|
-
}
|
|
2228
|
-
|
|
2229
|
-
// Bend direction of a 2-bone chain along axis e1: the pole direction projected off the axis. When
|
|
2230
|
-
// the limb points along its pole (a foreleg striking forward: elbow pole = backward) the projection
|
|
2231
|
-
// vanishes and its direction flips; there the last frame's bend direction takes over (continuous)
|
|
2232
|
-
bendDir(leg, k, pole, e1) {
|
|
2233
|
-
const raw = tv().copy(pole).addScaledVector(e1, -pole.dot(e1));
|
|
2234
|
-
const m = raw.length() / Math.max(1e-9, pole.length());
|
|
2235
|
-
const prev = leg.bend[k];
|
|
2236
|
-
const a = leg.bendInit[k] ? smooth(0.12, 0.4, m) : 1;
|
|
2237
|
-
raw.normalize();
|
|
2238
|
-
if (a < 1) raw.multiplyScalar(a).addScaledVector(tv().copy(prev).addScaledVector(e1, -prev.dot(e1)).normalize(), 1 - a).normalize();
|
|
2239
|
-
prev.copy(raw); leg.bendInit[k] = true;
|
|
2240
|
-
return raw;
|
|
2241
|
-
}
|
|
2242
|
-
|
|
2243
|
-
// Rotation (rad, about unit axis ax through a pivot at height pivotY) that keeps the point pivot + v
|
|
2244
|
-
// at or above floor. Of the two just-touching rotations the one nearest the previous correction
|
|
2245
|
-
// st[k] wins (no flipping between them); once the point is clear the correction relaxes to zero at
|
|
2246
|
-
// 4 rad/s (every angle between the root and zero keeps the point clear), so nothing snaps.
|
|
2247
|
-
clearAbout(ax, v, pivotY, floor, st, k, dt) {
|
|
2248
|
-
const last = st[k];
|
|
2249
|
-
const par = ax.dot(v), perpY = v.y - ax.y * par, crossY = ax.z * v.x - ax.x * v.z; // (ax x v).y
|
|
2250
|
-
if (pivotY + v.y >= floor) {
|
|
2251
|
-
const step = 4 * dt;
|
|
2252
|
-
const phiR = Math.abs(last) <= step ? 0 : last - Math.sign(last) * step;
|
|
2253
|
-
// (only as far as the point stays clear: from a large correction the arc back to zero can pass
|
|
2254
|
-
// below the floor, and a dead hereford's horn went 41 mm into a slope while it relaxed)
|
|
2255
|
-
if (pivotY + ax.y * par + perpY * Math.cos(phiR) + crossY * Math.sin(phiR) >= floor) { st[k] = phiR; return phiR; }
|
|
2256
|
-
}
|
|
2257
|
-
const R = Math.hypot(perpY, crossY);
|
|
2258
|
-
if (R < 1e-6) return last;
|
|
2259
|
-
const phi0 = Math.atan2(crossY, perpY), kk = (floor - pivotY - ax.y * par) / R;
|
|
2260
|
-
let phi = phi0;
|
|
2261
|
-
if (kk < 1) { const d = Math.acos(Math.max(-1, kk)); const p1 = angDiff(0, phi0 + d), p2 = angDiff(0, phi0 - d); phi = Math.abs(angDiff(last, p1)) < Math.abs(angDiff(last, p2)) ? p1 : p2; }
|
|
2262
|
-
// (at most 30 / sqrt(k) rad/s: the just-touching rotation grows as the square root of the depth
|
|
2263
|
-
// at its onset, and a dead cheetah's ear folded 0.3 rad in a 240 Hz frame as its head came down
|
|
2264
|
-
// on a slope, 87 rad/s*; the point may dip a frame's worth under its margin instead)
|
|
2265
|
-
const mr = 30 / this.cfg.sk * dt;
|
|
2266
|
-
phi = last + clamp(angDiff(last, phi), -mr, mr);
|
|
2267
|
-
st[k] = phi;
|
|
2268
|
-
return phi;
|
|
2269
|
-
}
|
|
2270
|
-
|
|
2271
|
-
// clearAbout for several points turned together (V[i], floors F[i], i < n): of the angles that keep
|
|
2272
|
-
// them all clear (the ends of each one's clear range, and zero), the one nearest the previous
|
|
2273
|
-
// correction st[k]; relaxing to zero at 4 rad/s while that keeps them all clear, at most
|
|
2274
|
-
// 30 / sqrt(k) rad/s; where no angle clears them all, the one that clears the deepest. (One
|
|
2275
|
-
// correction per set: each point with its own, relaxing on its own while another took over, a
|
|
2276
|
-
// sleeping boar's head rolled 80 deg and its throat went 11 cm into the ground)
|
|
2277
|
-
clearMulti(ax, V, F, n, pivotY, st, k, dt, acc = 0) {
|
|
2278
|
-
const last = st[k], C = this._cm;
|
|
2279
|
-
let clear0 = true, deep = -1, deepD = 0;
|
|
2280
|
-
for (let i = 0; i < n; i++) {
|
|
2281
|
-
const v = V[i], par = ax.dot(v), perpY = v.y - ax.y * par, crossY = ax.z * v.x - ax.x * v.z;
|
|
2282
|
-
const c0 = pivotY + ax.y * par, R = Math.hypot(perpY, crossY), kk = R > 1e-9 ? (F[i] - c0) / R : F[i] <= c0 ? -2 : 2;
|
|
2283
|
-
C[2 * i] = Math.atan2(crossY, perpY); C[2 * i + 1] = kk <= -1 ? 9 : kk >= 1 ? -1 : Math.acos(kk);
|
|
2284
|
-
const d = F[i] - pivotY - v.y;
|
|
2285
|
-
if (d > 0) { clear0 = false; if (d > deepD) { deepD = d; deep = i; } }
|
|
2286
|
-
}
|
|
2287
|
-
if (clear0) {
|
|
2288
|
-
const step = 4 * dt, phiR = Math.abs(last) <= step ? 0 : last - Math.sign(last) * step;
|
|
2289
|
-
if (this.clearAll(n, phiR)) { this._hcv[k] = (phiR - last) / dt; st[k] = phiR; return phiR; }
|
|
2290
|
-
}
|
|
2291
|
-
let phi = NaN, bd = Infinity;
|
|
2292
|
-
for (let i = 0; i < n; i++) {
|
|
2293
|
-
const hw = C[2 * i + 1];
|
|
2294
|
-
if (hw < 0 || hw === 9) continue;
|
|
2295
|
-
for (let e = -1; e <= 1; e += 2) {
|
|
2296
|
-
const a = angDiff(0, C[2 * i] + e * hw), dd = Math.abs(angDiff(last, a));
|
|
2297
|
-
if (dd < bd && this.clearAll(n, a)) { bd = dd; phi = a; }
|
|
2298
|
-
}
|
|
2299
|
-
}
|
|
2300
|
-
if (Number.isNaN(phi)) {
|
|
2301
|
-
if (deep < 0) return last;
|
|
2302
|
-
const hw = C[2 * deep + 1], p0 = C[2 * deep];
|
|
2303
|
-
if (hw < 0) phi = p0;
|
|
2304
|
-
else { const p1 = angDiff(0, p0 + hw), p2 = angDiff(0, p0 - hw); phi = Math.abs(angDiff(last, p1)) < Math.abs(angDiff(last, p2)) ? p1 : p2; }
|
|
2305
|
-
}
|
|
2306
|
-
if (acc > 0) {
|
|
2307
|
-
// (acc > 0: the speed is also gained and lost at acc / sqrt(k) rad/s^2, braking in time: a heavy
|
|
2308
|
-
// head's side correction, which as a dead hereford's head landed turned 0.13 rad one way for a
|
|
2309
|
-
// frame, then the other, and its jaw zig-zagged 7 % S)
|
|
2310
|
-
const vm = 30 / this.cfg.sk, A = acc / this.cfg.sk, sv = this._hcv, D = angDiff(last, phi);
|
|
2311
|
-
const vt = Math.sign(D) * Math.min(vm, Math.sqrt(2 * A * Math.abs(D)), Math.abs(D) / dt);
|
|
2312
|
-
sv[k] = clamp(vt, sv[k] - A * dt, sv[k] + A * dt);
|
|
2313
|
-
phi = last + sv[k] * dt;
|
|
2314
|
-
} else {
|
|
2315
|
-
const mr = 30 / this.cfg.sk * dt;
|
|
2316
|
-
phi = last + clamp(angDiff(last, phi), -mr, mr);
|
|
2317
|
-
}
|
|
2318
|
-
st[k] = phi;
|
|
2319
|
-
return phi;
|
|
2320
|
-
}
|
|
2321
|
-
// (does the angle phi keep every point clear that some angle can? clearMulti's ranges)
|
|
2322
|
-
clearAll(n, phi) {
|
|
2323
|
-
const C = this._cm;
|
|
2324
|
-
for (let i = 0; i < n; i++) {
|
|
2325
|
-
const hw = C[2 * i + 1];
|
|
2326
|
-
if (hw === 9 || hw < 0) continue;
|
|
2327
|
-
if (Math.abs(angDiff(C[2 * i], phi)) > hw + 1e-9) return false;
|
|
2328
|
-
}
|
|
2329
|
-
return true;
|
|
2330
|
-
}
|
|
2331
|
-
|
|
2332
|
-
// overreach (target distance / chain length) and its rate over stance, for the predictive lift-off
|
|
2333
|
-
setOverreach(leg, ov, dt) {
|
|
2334
|
-
leg.ovRate = leg.state === 'stance' && leg.stanceTime > 0 ? (ov - leg.overreach) / Math.max(dt, 1e-4) : 0;
|
|
2335
|
-
leg.overreach = ov;
|
|
2336
|
-
}
|
|
2337
|
-
|
|
2338
|
-
// toe-off on demand: a planted foot that the leg can no longer reach rolls up about the toe
|
|
2339
|
-
// (heel rises) just enough, so the toe stays where it was planted
|
|
2340
|
-
// (hooves: a sinking fetlock is soft tissue under load and only sinks as far as the leg reaches:
|
|
2341
|
-
// that part comes back up at once (leg.sinkLift, continuous with the reach), before any toe-off)
|
|
2342
|
-
toeOff(leg, ft, Mflat, Cf, T, M, latA) {
|
|
2343
|
-
const L = leg.def, J = this.J, fr = this.fr;
|
|
2344
|
-
const stance = leg.state === 'stance' && ft.free < 0.5 && leg.mode === 'gait';
|
|
2345
|
-
let toeW = stance ? smooth(0.3, 0.55, leg.u) : 0;
|
|
2346
|
-
// hooves: a sinking fetlock is soft tissue under load and only sinks as far as the leg reaches; it
|
|
2347
|
-
// comes back up (leg.sinkLift, spring-filtered by the caller) before any toe-off
|
|
2348
|
-
const sinkW = stance && Cf && ft.beta < 0 ? this.P.legGait : 0;
|
|
2349
|
-
leg.betaAdd = 0; leg.sinkLift = 0;
|
|
2350
|
-
const G = L.front
|
|
2351
|
-
? tv().copy(L.jShoulder).sub(J.chestMid).applyQuaternion(fr.qCh).add(fr.chestMid)
|
|
2352
|
-
: tv().copy(L.jHip).sub(J.lumbosacral).applyQuaternion(fr.qPel).add(fr.lumbo);
|
|
2353
|
-
// plantigrade hind: the metatarsus lies along the foot and rolls up with it, so the reach that
|
|
2354
|
-
// limits the stance is hip -> hock (the heel) as well as the pantograph's hip -> MTP
|
|
2355
|
-
const plantH = L.plant && !L.front;
|
|
2356
|
-
if (toeW <= 0 && sinkW <= 0 && !(plantH && stance)) return;
|
|
2357
|
-
let needM = 0;
|
|
2358
|
-
if (plantH && stance) {
|
|
2359
|
-
// (a plantigrade foot planted behind the hip may roll up early in its stance: after a landing
|
|
2360
|
-
// or a hard stop the gait can leave it there)
|
|
2361
|
-
const behind = (G.x - T.x) * ft.fwd.x + (G.z - T.z) * ft.fwd.z;
|
|
2362
|
-
toeW = Math.max(toeW, smooth(0.05 * L.len, 0.2 * L.len, behind));
|
|
2363
|
-
}
|
|
2364
|
-
if (plantH) {
|
|
2365
|
-
const Rm = (L.Av + L.L2) * 0.955;
|
|
2366
|
-
if (toeW > 0 && M.distanceToSquared(G) > Rm * Rm) needM = this.digitNeed(ft.beta, Mflat, Cf, T, latA, G, Rm * Rm);
|
|
2367
|
-
const Hf = footPoint(tv(), ft.c, latA, ft.n, ft.fwd, L.H);
|
|
2368
|
-
Mflat = Hf; M = tv(); placeDigit(ft.beta, Hf, null, T, latA, M, null);
|
|
2369
|
-
}
|
|
2370
|
-
const Rmax = L.front ? (L.Lh + L.Lr) * 0.95 + L.Lmc * 0.97 + 0.02 * L.len : plantH ? (L.L1 + L.L2) * 0.91 : (L.Av + L.L2) * 0.962; // (plantigrade: a margin for the low-passed roll)
|
|
2371
|
-
const rS = Rmax - 0.025 * L.len; // (the sink starts coming up a little before the toe-off)
|
|
2372
|
-
const d2 = M.distanceToSquared(G);
|
|
2373
|
-
if (needM > 0) leg.betaAdd = Math.min(needM, 45 * DEG) * this.P.legGait * toeW;
|
|
2374
|
-
if (d2 <= (sinkW > 0 ? rS * rS : Rmax * Rmax)) return;
|
|
2375
|
-
if (sinkW > 0) leg.sinkLift = Math.min(this.digitNeed(ft.beta, Mflat, Cf, T, latA, G, rS * rS), -ft.beta) * sinkW;
|
|
2376
|
-
if (toeW > 0 && d2 > Rmax * Rmax) {
|
|
2377
|
-
const need = this.digitNeed(ft.beta, Mflat, Cf, T, latA, G, Rmax * Rmax);
|
|
2378
|
-
leg.betaAdd = Math.max(leg.betaAdd, Math.max(0, Math.min(need - leg.sinkLift, 45 * DEG)) * this.P.legGait * toeW);
|
|
2379
|
-
}
|
|
2380
|
-
}
|
|
2381
|
-
|
|
2382
|
-
// smallest raise of the digit angle (from beta, up to 100 deg) that brings the foot's back point M
|
|
2383
|
-
// within sqrt(r2) of the girdle joint G (bisection)
|
|
2384
|
-
digitNeed(beta, Mflat, Cf, T, latA, G, r2) {
|
|
2385
|
-
const Mt = tv();
|
|
2386
|
-
let lo = beta, hi = Math.max(beta, 100 * DEG);
|
|
2387
|
-
placeDigit(hi, Mflat, Cf, T, latA, Mt, null);
|
|
2388
|
-
if (Mt.distanceToSquared(G) > r2) return hi - beta;
|
|
2389
|
-
for (let it = 0; it < 14; it++) {
|
|
2390
|
-
const mid = 0.5 * (lo + hi);
|
|
2391
|
-
placeDigit(mid, Mflat, Cf, T, latA, Mt, null);
|
|
2392
|
-
if (Mt.distanceToSquared(G) > r2) lo = mid; else hi = mid;
|
|
2393
|
-
}
|
|
2394
|
-
return hi - beta;
|
|
2395
|
-
}
|
|
2396
|
-
|
|
2397
|
-
// A middle joint (knee / elbow) at A + a e1 + b e2 that would sink into the ground is swung about
|
|
2398
|
-
// the limb axis e1 just enough to clear it (elbows / knees turn out, as in a lying cat). Rotates e2
|
|
2399
|
-
// in place; continuous (no rotation while the joint is clear).
|
|
2400
|
-
liftOffGround(A, e1, e2, a, b, leg, out) {
|
|
2401
|
-
// joint centre height that keeps the skin around an elbow / knee clear (at least its thickness)
|
|
2402
|
-
const r = Math.max(0.05 * this.cfg.k, 1.05 * leg.def.rE);
|
|
2403
|
-
const px = A.x + a * e1.x + b * e2.x, pz = A.z + a * e1.z + b * e2.z;
|
|
2404
|
-
const floor = this.terrainH(px, pz) + r;
|
|
2405
|
-
const c0 = A.y + a * e1.y;
|
|
2406
|
-
const last = leg.liftPhi || 0;
|
|
2407
|
-
if (b < 1e-6) { leg.liftPhi = 0; return; }
|
|
2408
|
-
const e3 = tv().crossVectors(e1, e2);
|
|
2409
|
-
if (c0 + b * e2.y >= floor) {
|
|
2410
|
-
// clear without rotating: relax a rotation left from earlier frames back to zero at a limited
|
|
2411
|
-
// rate (every angle between the last root and zero keeps the joint clear), no snap
|
|
2412
|
-
if (last === 0) return;
|
|
2413
|
-
const step = 4 * this.frameDt;
|
|
2414
|
-
const phi = Math.abs(last) <= step ? 0 : last - Math.sign(last) * step;
|
|
2415
|
-
leg.liftPhi = phi;
|
|
2416
|
-
if (phi !== 0) e2.multiplyScalar(Math.cos(phi)).addScaledVector(e3, Math.sin(phi)).normalize();
|
|
2417
|
-
return;
|
|
2418
|
-
}
|
|
2419
|
-
const R = b * Math.hypot(e2.y, e3.y);
|
|
2420
|
-
if (R < 1e-9) return;
|
|
2421
|
-
const phi0 = Math.atan2(e3.y, e2.y);
|
|
2422
|
-
const k = (floor - c0) / R;
|
|
2423
|
-
let phi;
|
|
2424
|
-
if (k >= 1) phi = phi0;
|
|
2425
|
-
else {
|
|
2426
|
-
const d = Math.acos(Math.max(-1, k));
|
|
2427
|
-
const p1 = angDiff(0, phi0 + d), p2 = angDiff(0, phi0 - d);
|
|
2428
|
-
// the side the joint swung to last frame wins (no flipping between the two solutions); from
|
|
2429
|
-
// rest, the smaller rotation, unless that swings the joint in under the body (out: the leg's
|
|
2430
|
-
// outward side) and the other does not (a lying cow's elbow swung in under its chest, 23 cm
|
|
2431
|
-
// across from its shoulder, and turned the cannon over as it stood up: 578 rad/s*)
|
|
2432
|
-
if (last !== 0 || !out) phi = Math.abs(angDiff(last, p1)) < Math.abs(angDiff(last, p2)) ? p1 : p2;
|
|
2433
|
-
else {
|
|
2434
|
-
const o2 = e2.dot(out), o3 = e3.dot(out);
|
|
2435
|
-
const l1 = o2 * Math.cos(p1) + o3 * Math.sin(p1), l2 = o2 * Math.cos(p2) + o3 * Math.sin(p2);
|
|
2436
|
-
phi = Math.abs(p1) <= Math.abs(p2) ? (l1 < -0.5 && l2 > l1 ? p2 : p1) : (l2 < -0.5 && l1 > l2 ? p1 : p2);
|
|
2437
|
-
}
|
|
2438
|
-
}
|
|
2439
|
-
// (at most 30 / sqrt(k) rad/s: near the highest point the rotation can lift the joint to, the root
|
|
2440
|
-
// swings fast with the geometry, and a retriever's elbow, held 42 mm up while it rose from its
|
|
2441
|
-
// sleep, swung 0.19 rad in a frame and turned the cannon over, 319 rad/s*)
|
|
2442
|
-
// (and at most 2 rad out of the limb's bend plane: further, the rotation that just lifts the
|
|
2443
|
-
// joint runs up against the highest point it can reach, where the elbow of that retriever swung
|
|
2444
|
-
// about; a curled-up wolf's elbow needs 1.5, and a cap at 1.6 made the retriever's elbow stop
|
|
2445
|
-
// dead against it, 3.3 % S of jitter)
|
|
2446
|
-
const mr = 30 / this.cfg.sk * this.frameDt;
|
|
2447
|
-
phi = clamp(last + clamp(angDiff(last, phi), -mr, mr), -2, 2);
|
|
2448
|
-
leg.liftPhi = phi;
|
|
2449
|
-
const c = Math.cos(phi), sn = Math.sin(phi);
|
|
2450
|
-
e2.multiplyScalar(c).addScaledVector(e3, sn).normalize();
|
|
2451
|
-
}
|
|
2452
|
-
|
|
2453
|
-
// keep the last two solved joint chains (world) for LOD 2 half-rate IK
|
|
2454
|
-
// (f: scapula top of a foreleg, g: coffin joint of a hoof)
|
|
2455
|
-
storeLeg(leg, a, b, c, d, e, f, g) {
|
|
2456
|
-
const jp = leg.jp, jp0 = leg.jp0, jpp = leg.jpp;
|
|
2457
|
-
for (let i = 0; i < 7; i++) { jpp[i].copy(jp0[i]); jp0[i].copy(jp[i]); }
|
|
2458
|
-
jp[0].copy(a); jp[1].copy(b); jp[2].copy(c); jp[3].copy(d); jp[4].copy(e); if (f) jp[5].copy(f); if (g) jp[6].copy(g);
|
|
2459
|
-
leg.solved = Math.min(3, leg.solved + 1);
|
|
2460
|
-
leg.sf[0] = leg.sf[1]; leg.sf[1] = leg.sf[2]; leg.sf[2] = this.frame;
|
|
2461
|
-
leg.latRef = leg.latRef || new THREE.Vector3();
|
|
2462
|
-
}
|
|
2463
|
-
|
|
2464
|
-
// LOD 2 in-between frame: extrapolate the chain by one frame and re-anchor it on the girdle
|
|
2465
|
-
extrapolateLeg(leg) {
|
|
2466
|
-
const L = leg.def, S = L.S, J = this.J, fr = this.fr;
|
|
2467
|
-
const jp = leg.jp, jp0 = leg.jp0;
|
|
2468
|
-
// quadratic through the last three half-rate solves (frames -4, -2, 0), evaluated one frame on
|
|
2469
|
-
const jpp = leg.jpp;
|
|
2470
|
-
const P = (i) => tv().copy(jp[i]).multiplyScalar(1.875).addScaledVector(jp0[i], -1.25).addScaledVector(jpp[i], 0.375);
|
|
2471
|
-
const top = L.front
|
|
2472
|
-
? tv().copy(L.jShoulder).sub(J.chestMid).applyQuaternion(fr.qCh).add(fr.chestMid)
|
|
2473
|
-
: tv().copy(L.jHip).sub(J.lumbosacral).applyQuaternion(fr.qPel).add(fr.lumbo);
|
|
2474
|
-
const p0 = P(0), p1 = P(1), p2 = P(2), p3 = P(3), p4 = P(4);
|
|
2475
|
-
const corr = tv().subVectors(top, p0);
|
|
2476
|
-
p0.add(corr); p1.addScaledVector(corr, 0.66); p2.addScaledVector(corr, 0.33);
|
|
2477
|
-
const lat = tv(1, 0, 0).applyQuaternion(fr.qB);
|
|
2478
|
-
if (L.front) {
|
|
2479
|
-
const p5 = P(5).add(corr);
|
|
2480
|
-
this.setBoneF(L.n.scapula, p5, tv().subVectors(p0, p5), lat);
|
|
2481
|
-
this.setBoneF(L.n.humerus, p0, tv().subVectors(p1, p0), lat);
|
|
2482
|
-
this.setBoneF(L.n.radius, p1, tv().subVectors(p2, p1), lat);
|
|
2483
|
-
this.setBoneF(L.n.metacarpus, p2, tv().subVectors(p3, p2), lat);
|
|
2484
|
-
} else {
|
|
2485
|
-
this.setBoneF(L.n.femur, p0, tv().subVectors(p1, p0), lat);
|
|
2486
|
-
this.setBoneF(L.n.tibia, p1, tv().subVectors(p2, p1), lat);
|
|
2487
|
-
this.setBoneF(L.n.metatarsus, p2, tv().subVectors(p3, p2), lat);
|
|
2488
|
-
}
|
|
2489
|
-
const paw = L.front ? L.n.fpaw : L.n.hpaw;
|
|
2490
|
-
if (L.hoof) {
|
|
2491
|
-
// pastern (fetlock -> coffin joint) and hoof (coffin joint -> toe)
|
|
2492
|
-
const p6 = P(6);
|
|
2493
|
-
this.setBoneF(paw, p3, tv().subVectors(p6, p3), lat);
|
|
2494
|
-
this.setBoneF(L.hoof, p6, tv().subVectors(p4, p6), lat);
|
|
2495
|
-
} else this.setBoneF(paw, p3, tv().subVectors(p4, p3), lat);
|
|
2496
|
-
leg.contact.copy(leg.state === 'stance' ? leg.plant : p4);
|
|
2497
|
-
}
|
|
2498
|
-
|
|
2499
|
-
// ----------------------------------------------------------------- neck & head
|
|
2500
|
-
// stride nod of the head: { a: amplitude (share of the neck length), y: shape -1..1 (1 = lowest) }
|
|
2501
|
-
strideNod(vn) {
|
|
2502
|
-
const g = this.gait, P = this.P, cfg = this.cfg, out = this._nod || (this._nod = { a: 0, y: 0 });
|
|
2503
|
-
const tb = cfg.head.nod;
|
|
2504
|
-
const mv = smooth(0.05, 0.5, vn) * P.gaitW * (1 - P.headW) * (1 - 0.6 * (P.lookW || 0)) * this._bobGate;
|
|
2505
|
-
const lg = smooth(0.3, 0.8, this.P.legGait);
|
|
2506
|
-
if (!tb || mv * lg < 1e-3) { out.a = 0; out.y = 0; return out; }
|
|
2507
|
-
const gal = g.gallop, trot = smooth(0.35, 0.6, 1 - g.D) * (1 - gal);
|
|
2508
|
-
out.a = (tb[0] * (1 - trot - gal) + tb[1] * trot + tb[2] * gal) * mv * lg;
|
|
2509
|
-
const c0 = g.off[0] + 0.5 * g.DL[0] + (tb[3] || 0), c1 = g.off[1] + 0.5 * g.DL[1] + (tb[3] || 0);
|
|
2510
|
-
const two = Math.cos(4 * Math.PI * (this.phase - c0));
|
|
2511
|
-
const cm = c0 + 0.5 * d01(c0, c1);
|
|
2512
|
-
const one = Math.cos(2 * Math.PI * (this.phase - cm));
|
|
2513
|
-
out.y = lerp(two, one, gal);
|
|
2514
|
-
return out;
|
|
2515
|
-
}
|
|
2516
|
-
|
|
2517
|
-
solveHead(dt) {
|
|
2518
|
-
const cfg = this.cfg, J = this.J, fr = this.fr, g = this.gait, P = this.P, inp = this.input;
|
|
2519
|
-
const vn = this.speed / cfg.sq, s = cfg.k, gal = g.gallop;
|
|
2520
|
-
const crouch = inp.crouch || 0;
|
|
2521
|
-
const moving = smooth(0.05, 0.9, vn);
|
|
2522
|
-
const carry = tableAt(cfg.headCarry, vn, this._carry);
|
|
2523
|
-
// how much the head is held still in space (vs. carried by the chest): rises with speed
|
|
2524
|
-
const stab = clamp(tableAt(cfg.headStab, vn, this._stab)[0] * P.gaitW, 0, 0.95);
|
|
2525
|
-
const neckRaise = carry[0] - crouch * 0.12 * s + P.headRaise * s;
|
|
2526
|
-
const reach = carry[1] + crouch * 0.03 * s + P.neckReach * s;
|
|
2527
|
-
const qCh = fr.qCh, base = fr.neckBase, qHeading = fr.qHeading;
|
|
2528
|
-
// rigid target: carried by the chest (mostly), levelled a little at speed
|
|
2529
|
-
const qN = tqc(qHeading).slerp(qCh, lerp(0.9, 0.6, gal));
|
|
2530
|
-
const rigid = tv().copy(J.occiput).sub(J.neckBase).applyQuaternion(qN).add(base).addScaledVector(fr.F, reach);
|
|
2531
|
-
rigid.y += neckRaise;
|
|
2532
|
-
if (P.headLocalW > 0) rigid.add(tv().copy(P.headLocal).multiplyScalar(s * P.headLocalW).applyQuaternion(fr.qB));
|
|
2533
|
-
// stabilised target: low-pass filtered in the heading frame, relative to the ground under the body
|
|
2534
|
-
const qInv = tqc(qHeading).invert();
|
|
2535
|
-
const ref = tv(this.pos.x, this.pos.y + this.lift, this.pos.z);
|
|
2536
|
-
const rel = tv().subVectors(rigid, ref).applyQuaternion(qInv);
|
|
2537
|
-
if (!this._headInit) {
|
|
2538
|
-
this.hx.reset(rel.x); this.hy.reset(rel.y); this.hz.reset(rel.z);
|
|
2539
|
-
this._headInit = true;
|
|
2540
|
-
}
|
|
2541
|
-
const w = lerp(7.5, 5.5, gal);
|
|
2542
|
-
const relS = tv(this.hx.step(rel.x, w, dt), this.hy.step(rel.y, w * 0.85, dt), this.hz.step(rel.z, w, dt));
|
|
2543
|
-
const stabP = relS.applyQuaternion(qHeading).add(ref);
|
|
2544
|
-
const Otgt = tv().copy(rigid).lerp(stabP, stab);
|
|
2545
|
-
// stride nod: the head and neck dip as the forelegs take the weight (walk and trot: at each
|
|
2546
|
-
// foreleg's mid stance, twice a stride; canter and gallop: once, a pump down and forward as the
|
|
2547
|
-
// forelegs land and back up through the hind stance). Added after the stabilisation, whose
|
|
2548
|
-
// filter would swallow a 2 Hz nod. cfg.head.nod: [walk, trot, gallop] amplitude / neck length
|
|
2549
|
-
const nod = this.strideNod(vn);
|
|
2550
|
-
// (braking: the head comes up and back, see pose)
|
|
2551
|
-
if (this.brake > 0.01) { Otgt.y += 0.09 * this.brake * cfg.neckLen; Otgt.addScaledVector(fr.F, -0.04 * this.brake * cfg.neckLen); }
|
|
2552
|
-
if (nod.a !== 0) {
|
|
2553
|
-
Otgt.y -= nod.a * nod.y * cfg.neckLen;
|
|
2554
|
-
Otgt.addScaledVector(fr.F, 0.45 * nod.a * nod.y * cfg.neckLen * gal);
|
|
2555
|
-
}
|
|
2556
|
-
// posture head target (world occiput position), e.g. mouth to the ground
|
|
2557
|
-
if (P.headW > 0.001) Otgt.lerp(P.headPos, P.headW);
|
|
2558
|
-
// one-arc neck of constant length, leaving the chest along its own neck direction
|
|
2559
|
-
const l1 = J.neckBase.distanceTo(J.neckMid), Ln = cfg.neckLen;
|
|
2560
|
-
const tChest = tv().copy(J.neckMid).sub(J.neckBase).normalize().applyQuaternion(qCh);
|
|
2561
|
-
let cdir = tv().subVectors(Otgt, base).normalize();
|
|
2562
|
-
const tB = tv().copy(tChest).lerp(cdir, cfg.head.neckPivot).normalize();
|
|
2563
|
-
let th = Math.acos(clamp(tB.dot(cdir), -1, 1));
|
|
2564
|
-
const thMax = cfg.head.neckBend * DEG;
|
|
2565
|
-
if (th > thMax) {
|
|
2566
|
-
const ax = tv().crossVectors(tB, cdir).normalize();
|
|
2567
|
-
cdir = tv().copy(tB).applyAxisAngle(ax, thMax);
|
|
2568
|
-
th = thMax;
|
|
2569
|
-
}
|
|
2570
|
-
const nC = tv().copy(cdir).addScaledVector(tB, -cdir.dot(tB));
|
|
2571
|
-
const nCl = nC.length();
|
|
2572
|
-
const mid = arcPoint(tv(), base, tB, nC, nCl, th, Ln, l1);
|
|
2573
|
-
const occ = arcPoint(tv(), base, tB, nC, nCl, th, Ln, Ln);
|
|
2574
|
-
const latRef = tv(1, 0, 0).applyQuaternion(qCh);
|
|
2575
|
-
this.setBoneF('neck1', base, tv().subVectors(mid, base), latRef);
|
|
2576
|
-
this.setBoneF('neck2', mid, tv().subVectors(occ, mid), latRef);
|
|
2577
|
-
|
|
2578
|
-
// head orientation: gaze-stabilised (level, along heading + look target), with a little of the
|
|
2579
|
-
// chest's pitch and roll let through at low speed so the walk does not look robotic
|
|
2580
|
-
let yawT = 0, pitchT = carry[2] + crouch * 0.12;
|
|
2581
|
-
const look = inp.look || P.look;
|
|
2582
|
-
if (look && P.lookW > 0.01) {
|
|
2583
|
-
const to = tv().subVectors(look, occ);
|
|
2584
|
-
const want = Math.atan2(to.x, to.z);
|
|
2585
|
-
const hd = cfg.head;
|
|
2586
|
-
yawT = clamp(angDiff(this.heading, want), -hd.yaw, hd.yaw) * P.lookW;
|
|
2587
|
-
pitchT = lerp(pitchT, clamp(-Math.atan2(to.y, Math.hypot(to.x, to.z)), -hd.pitchUp, hd.pitchDown), P.lookW);
|
|
2588
|
-
this.lookTarget.copy(look);
|
|
2589
|
-
this.state.lookTarget = this.lookTarget;
|
|
2590
|
-
} else this.state.lookTarget = null;
|
|
2591
|
-
// the head leads a turn: it turns toward the commanded heading at once, before the body (which
|
|
2592
|
-
// follows over the next half second), and with the turn rate (it followed the turn rate only and
|
|
2593
|
-
// trailed the chest into a turn)
|
|
2594
|
-
const steer = inp.follow ? 0 : 0.5 * angDiff(this.heading, this.wantHeading ?? this.heading);
|
|
2595
|
-
yawT += clamp(this.yawRate * 0.22 + steer, -0.5, 0.5) + P.headYaw;
|
|
2596
|
-
pitchT += P.headPitch;
|
|
2597
|
-
const hw = P.headOmega;
|
|
2598
|
-
const yaw = this.lookYaw.step(yawT, 5.5 * hw, dt);
|
|
2599
|
-
const hp = this.headPitch.step(pitchT, 5.5 * hw, dt);
|
|
2600
|
-
const hr = this.headRollS.step(P.headRoll, 6 * hw, dt);
|
|
2601
|
-
const qHead = tq().setFromAxisAngle(UP, this.heading + yaw).multiply(tqa(X1, hp + 0.4 * nod.a * nod.y)).multiply(tqa(Z1, -0.25 * this.bank.x + hr));
|
|
2602
|
-
const carried = tqc(qCh).multiply(qInv).multiply(qHead);
|
|
2603
|
-
qHead.copy(carried.slerp(qHead, lerp(0.7, 0.95, clamp(stab / 0.62, 0, 1)) * (1 - P.headLimp)));
|
|
2604
|
-
// the nose / chin never enter the ground: pitch the head up about the occiput just enough
|
|
2605
|
-
// ... and the ear bases and the skin to either side of the head and jaw (cheeks, jaw, horns,
|
|
2606
|
-
// antlers: cfg.headSide) never enter it (a head lying on its side): roll the head back up about
|
|
2607
|
-
// its own long axis just enough (the sides only near the ground, or while a correction relaxes)
|
|
2608
|
-
// (then the nose and chin once more: rolling the head up onto a horn can put the chin down)
|
|
2609
|
-
const HS = cfg.headSide;
|
|
2610
|
-
// (near the ground: first against the ground under the body, which costs nothing, then under the head)
|
|
2611
|
-
const hc = this._hc, sides = HS && (hc[6] !== 0 || hc[7] !== 0 || hc[8] !== 0 || hc[9] !== 0 ||
|
|
2612
|
-
(occ.y - this.pos.y < cfg.headSideReach + 0.5 * cfg.legLen && occ.y - this.terrainH(occ.x, occ.z) < cfg.headSideReach + 0.02 * cfg.k));
|
|
2613
|
-
for (let k = 0; k < 8; k++) {
|
|
2614
|
-
if ((k === 2 || k === 3) && !cfg.earLocal) continue;
|
|
2615
|
-
if (k >= 4 && !sides) break;
|
|
2616
|
-
const nk = k >= 6 ? k - 6 : k;
|
|
2617
|
-
const ax = tv(nk < 2 ? 1 : 0, 0, nk < 2 ? 0 : 1).applyQuaternion(qHead);
|
|
2618
|
-
let phi;
|
|
2619
|
-
if (k === 4 || k === 5) {
|
|
2620
|
-
// a side: its cheek, jaw and horn points together (one correction, clearMulti)
|
|
2621
|
-
// (each point's floor under where last frame's correction put it: on a slope a long horn swings
|
|
2622
|
-
// well away from where it was before the roll)
|
|
2623
|
-
const pts = HS[k - 4], V = this._cmV, F = this._cmF, w = tv(), l = this._hc[k + 2];
|
|
2624
|
-
for (let j = 0; j < pts.length; j++) {
|
|
2625
|
-
V[j].copy(pts[j]).applyQuaternion(qHead);
|
|
2626
|
-
w.copy(V[j]).applyAxisAngle(ax, l);
|
|
2627
|
-
F[j] = this.terrainH(occ.x + w.x, occ.z + w.z) + 0.012 * cfg.k;
|
|
2628
|
-
}
|
|
2629
|
-
phi = this.clearMulti(ax, V, F, pts.length, occ.y, this._hc, k + 2, dt, 600);
|
|
2630
|
-
} else {
|
|
2631
|
-
const loc = nk === 0 ? cfg.noseLocal : nk === 1 ? cfg.chinLocal : cfg.earLocal[k - 2];
|
|
2632
|
-
const v = tv().copy(loc).applyQuaternion(qHead);
|
|
2633
|
-
const floor = this.terrainH(occ.x + v.x, occ.z + v.z) + (nk < 2 ? 0.012 : 0.03) * cfg.k;
|
|
2634
|
-
phi = this.clearAbout(ax, v, occ.y, floor, this._hc, k < 4 ? k : k + 2, dt);
|
|
2635
|
-
}
|
|
2636
|
-
if (phi !== 0) qHead.premultiply(tqa(ax, phi));
|
|
2637
|
-
}
|
|
2638
|
-
this.setBoneQ('head', occ, qHead, ONE);
|
|
2639
|
-
this.headPose.position.copy(occ);
|
|
2640
|
-
this.headPose.quaternion.copy(qHead);
|
|
2641
|
-
// jaw: panting, actions (bite, chew, lap)
|
|
2642
|
-
const pant = cfg.breath.pant ? smooth(0.15, 0.6, this.heat) * (vn < 6 ? 1 : 0.3) : 0;
|
|
2643
|
-
const jawT = pant * (0.16 + 0.08 * Math.sin(this.breath)) + P.jaw;
|
|
2644
|
-
const ja = this.jaw.step(jawT, P.jawOmega, dt);
|
|
2645
|
-
if (cfg.hasJaw) {
|
|
2646
|
-
const hinge = tv().copy(J.jawHinge).sub(J.occiput).applyQuaternion(qHead).add(occ);
|
|
2647
|
-
this.setBoneQ('jaw', hinge, tqc(qHead).multiply(tqa(X1, ja)), ONE);
|
|
2648
|
-
}
|
|
2649
|
-
if (cfg.hasEars) this.solveEars(dt, qHead, occ, look);
|
|
2650
|
-
}
|
|
2651
|
-
|
|
2652
|
-
solveEars(dt, qHead, occ, look) {
|
|
2653
|
-
const cfg = this.cfg, J = this.J, P = this.P, E = cfg.ears, t = this.time;
|
|
2654
|
-
const vn = this.speed / cfg.sq;
|
|
2655
|
-
const lod = this.lod;
|
|
2656
|
-
for (let k = 0; k < 2; k++) {
|
|
2657
|
-
const S = k === 0 ? 'L' : 'R', s = k === 0 ? 1 : -1, ER = this.ears[k];
|
|
2658
|
-
const sp = k === 0 ? this.earL : this.earR, sw = k === 0 ? this.earSwL : this.earSwR;
|
|
2659
|
-
const tw = lod ? 0 : E.twitch * P.earTwitch * ((Math.sin(t * 0.7 + s * 3) > 0.985 ? 0.5 : 0) + 0.08 * Math.sin(t * 1.3 + s));
|
|
2660
|
-
const fs = E.flattenSpeeds || FLATTEN_SPEEDS;
|
|
2661
|
-
const back = smooth(fs[0], fs[1], vn) * E.speedFlatten * P.gaitW;
|
|
2662
|
-
const a = sp.step(E.rest[0] + (back + tw + P.earFlat * 1.1) * E.mobility, 18, dt);
|
|
2663
|
-
// swivel the pinna toward the look target
|
|
2664
|
-
let swT = 0;
|
|
2665
|
-
if (look && E.prick) {
|
|
2666
|
-
const to = tv().subVectors(look, occ);
|
|
2667
|
-
const rel = clamp(angDiff(this.heading, Math.atan2(to.x, to.z)), -1.5, 1.5);
|
|
2668
|
-
swT = clamp(rel * s * 0.6, -1, 1) * E.prick;
|
|
2669
|
-
}
|
|
2670
|
-
swT += P.earSwivel * s;
|
|
2671
|
-
const swv = sw.step(swT * (1 - P.earFlat), 10, dt);
|
|
2672
|
-
const base = tv().copy(ER.base).sub(J.occiput).applyQuaternion(qHead).add(occ);
|
|
2673
|
-
const earAx = tv().subVectors(ER.tip, ER.base).normalize();
|
|
2674
|
-
const q = tqc(qHead).multiply(tqa(X1, a)).multiply(tqa(Z1, -s * (a * 0.4 + E.rest[1]))).multiply(tqa(earAx, swv));
|
|
2675
|
-
// the pinna never enters the ground (a head lying on its side): fold it back or forward along
|
|
2676
|
-
// the head just enough (axis across the ear and the head's long axis: never parallel to the ear)
|
|
2677
|
-
const v = tv().subVectors(ER.tip, ER.base).applyQuaternion(q);
|
|
2678
|
-
const ax = tv().crossVectors(v, tv(0, 0, 1).applyQuaternion(qHead)).normalize();
|
|
2679
|
-
// (and the pinna's skin tip and rim with it, ER.pts: one correction, clearMulti)
|
|
2680
|
-
let phi;
|
|
2681
|
-
if (ER.pts && (this._hc[4 + k] !== 0 || (base.y - this.pos.y < ER.reach + 0.5 * cfg.legLen && base.y - this.terrainH(base.x, base.z) < ER.reach + 0.02 * cfg.k))) {
|
|
2682
|
-
// (each point's floor under where last frame's correction put it)
|
|
2683
|
-
const V = this._cmV, F = this._cmF, w = tv(), l = this._hc[4 + k];
|
|
2684
|
-
for (let j = 0; j <= EAR_PTS; j++) {
|
|
2685
|
-
if (j === 0) V[0].copy(v); else V[j].copy(ER.pts[j - 1]).applyQuaternion(q);
|
|
2686
|
-
w.copy(V[j]).applyAxisAngle(ax, l);
|
|
2687
|
-
F[j] = this.terrainH(base.x + w.x, base.z + w.z) + (j === 0 ? 0.008 : 0.006) * cfg.k;
|
|
2688
|
-
}
|
|
2689
|
-
phi = this.clearMulti(ax, V, F, EAR_PTS + 1, base.y, this._hc, 4 + k, dt);
|
|
2690
|
-
} else phi = this.clearAbout(ax, v, base.y, this.terrainH(base.x + v.x, base.z + v.z) + 0.008 * cfg.k, this._hc, 4 + k, dt);
|
|
2691
|
-
if (phi !== 0) q.premultiply(tqa(ax, phi));
|
|
2692
|
-
this.setBoneQ(ER.name, base, q, ONE);
|
|
2693
|
-
}
|
|
2694
|
-
}
|
|
2695
|
-
|
|
2696
|
-
// ----------------------------------------------------------------- tail
|
|
2697
|
-
// Carriage by speed from the species table; stride sway; idle life (flick / wag / swish);
|
|
2698
|
-
// the cheetah tail-use model in turns and speed changes; each segment is a damped angular spring
|
|
2699
|
-
// toward a target bend relative to its parent, so motion ripples from base to tip; ground contact
|
|
2700
|
-
// is a hard constraint.
|
|
2701
|
-
solveTail(dt) {
|
|
2702
|
-
const cfg = this.cfg, T = cfg.tail, tl = this.tail, n = tl.n, g = this.gait, P = this.P, fr = this.fr;
|
|
2703
|
-
const v = this.speed, vn = v / cfg.sq, gal = g.gallop * P.gaitW, s = cfg.k;
|
|
2704
|
-
const moving = smooth(0.05, 1.0, vn) * P.gaitW;
|
|
2705
|
-
const t = this.time;
|
|
2706
|
-
const pitch = tl.pitch, yaw = tl.yaw;
|
|
2707
|
-
tableAt(cfg.tailCarry, vn, tl.carry);
|
|
2708
|
-
for (let i = 0; i < n; i++) { pitch[i] = tl.carry[i]; yaw[i] = 0; }
|
|
2709
|
-
// drive signals
|
|
2710
|
-
const aFwd = (v - tl.prevV) / Math.max(dt, 1e-4);
|
|
2711
|
-
tl.prevV = v;
|
|
2712
|
-
tl.aFwdS = expTo(tl.aFwdS, clamp(aFwd, -14, 14), dt, 0.16);
|
|
2713
|
-
const aLat = v * this.yawRate;
|
|
2714
|
-
tl.aLatFast = expTo(tl.aLatFast, aLat, dt, 0.04);
|
|
2715
|
-
tl.aLatSlow = expTo(tl.aLatSlow, aLat, dt, 0.14);
|
|
2716
|
-
// stride sway at walk / trot
|
|
2717
|
-
const swayA = T.sway * DEG * moving * (1 - 0.55 * smooth(1.6, 3.5, vn)) * (1 - gal);
|
|
2718
|
-
for (let i = 0; i < n; i++) {
|
|
2719
|
-
const k = i * 10 / n;
|
|
2720
|
-
yaw[i] += swayA * (0.35 + 0.12 * k) * Math.sin(TAU * this.phase - 0.42 * k);
|
|
2721
|
-
pitch[i] += 0.06 * gal * Math.sin(TAU * (this.phase + 0.3) - 0.3 * k);
|
|
2722
|
-
}
|
|
2723
|
-
// curl over the back (distal segments bend up)
|
|
2724
|
-
if (T.curl) for (let i = 0; i < n; i++) pitch[i] += T.curl * DEG * Math.pow(i / Math.max(1, n - 1), 1.5);
|
|
2725
|
-
// idle life
|
|
2726
|
-
const idle = (1 - moving) * P.tailIdle;
|
|
2727
|
-
if (idle > 0.01) {
|
|
2728
|
-
const wander = Math.sin(t * 0.31) * 0.6 + Math.sin(t * 0.137 + 1.3) * 0.4;
|
|
2729
|
-
for (let i = 0; i < n; i++) yaw[i] += idle * 14 * DEG * wander * (0.25 + i / n);
|
|
2730
|
-
if (T.flick) {
|
|
2731
|
-
tl.flickT -= dt;
|
|
2732
|
-
if (tl.flickT <= 0) { tl.flickT = 3.5 + this.rand() * 6; tl.flickAmp = (this.rand() < 0.5 ? -1 : 1) * (0.6 + this.rand() * 0.4); tl.flickAge = 0; }
|
|
2733
|
-
tl.flickAge += dt;
|
|
2734
|
-
const fk = Math.exp(-tl.flickAge * 3.2) * Math.sin(Math.min(tl.flickAge * 9, Math.PI)) * T.flick;
|
|
2735
|
-
const i0 = Math.floor(n * 0.6);
|
|
2736
|
-
for (let i = i0; i < n; i++) { const r = (i - i0 + 1) / (n - i0); yaw[i] += idle * fk * tl.flickAmp * 22 * DEG * r; pitch[i] += idle * fk * 12 * DEG * r; }
|
|
2737
|
-
}
|
|
2738
|
-
if (T.swish) {
|
|
2739
|
-
tl.swishT -= dt;
|
|
2740
|
-
if (tl.swishT <= 0) { tl.swishT = 2 + this.rand() * 5; tl.swishAmp = (this.rand() < 0.5 ? -1 : 1); tl.swishAge = 0; }
|
|
2741
|
-
tl.swishAge += dt;
|
|
2742
|
-
const sk = Math.sin(Math.min(tl.swishAge * 2.4, Math.PI)) * T.swish;
|
|
2743
|
-
for (let i = 0; i < n; i++) yaw[i] += idle * sk * tl.swishAmp * 18 * DEG * (0.3 + i / n);
|
|
2744
|
-
}
|
|
2745
|
-
}
|
|
2746
|
-
const wag = Math.max(T.wag * (1 - moving * 0.6), P.tailWag);
|
|
2747
|
-
if (wag > 0.01) for (let i = 0; i < n; i++) yaw[i] += wag * 0.5 * Math.sin(t * TAU * 2.6 - i * 0.25) * (0.4 + i / n);
|
|
2748
|
-
// cheetah tail-use model: into the turn, then swung to the outside; cone in sustained turns;
|
|
2749
|
-
// acceleration swings it down, braking up
|
|
2750
|
-
if (T.balance) {
|
|
2751
|
-
const turnIn = 0.18 * (tl.aLatFast - tl.aLatSlow);
|
|
2752
|
-
const turnOut = -0.066 * tl.aLatSlow;
|
|
2753
|
-
const side = clamp((turnIn + turnOut) * smooth(2, 8, vn), -1.1, 1.1) * T.balance * P.gaitW;
|
|
2754
|
-
const cone = clamp(Math.abs(tl.aLatSlow) / 13, 0, 1) * 0.2 * gal * T.balance;
|
|
2755
|
-
for (let i = 0; i < n; i++) {
|
|
2756
|
-
const k = i * 10 / n;
|
|
2757
|
-
yaw[i] += side * (0.55 + 0.06 * k) + cone * Math.sin(TAU * this.phase) * (0.5 + 0.06 * k);
|
|
2758
|
-
pitch[i] += Math.abs(side) * 0.3 + cone * Math.cos(TAU * this.phase) * Math.sign(tl.aLatSlow || 1) * 0.5;
|
|
2759
|
-
}
|
|
2760
|
-
const pAcc = clamp(-tl.aFwdS * 0.042, -0.34, 0.55) * smooth(0.5, 3, vn + Math.abs(tl.aFwdS) * 0.3) * T.balance;
|
|
2761
|
-
for (let i = 0; i < n; i++) pitch[i] += pAcc * (1 - 0.35 * i / Math.max(1, n - 1));
|
|
2762
|
-
}
|
|
2763
|
-
// posture: lift, drop to the ground, curl around the body
|
|
2764
|
-
for (let i = 0; i < n; i++) {
|
|
2765
|
-
const r = i / Math.max(1, n - 1);
|
|
2766
|
-
pitch[i] += P.tailLift * (1 - 0.5 * r);
|
|
2767
|
-
if (P.tailGround > 0) pitch[i] = lerp(pitch[i], -1.35 + 1.1 * r * r, P.tailGround);
|
|
2768
|
-
yaw[i] += P.tailSide * (0.05 + Math.pow(r, 1.3)); // cumulative: the tail curls around
|
|
2769
|
-
yaw[i] += P.tailCurl * (1 - Math.pow(1 - r, 3)); // wrapped: turns near the root, then runs on
|
|
2770
|
-
}
|
|
2771
|
-
// frame: follow the pelvis near the base, stabilised in space at speed
|
|
2772
|
-
const qT = tqc(fr.qHeading).slerp(fr.qPel, lerp(0.8, 0.4, gal) + (1 - P.gaitW) * 0.2);
|
|
2773
|
-
const back = tv(0, 0, -1).applyQuaternion(qT), upT = tv(0, 1, 0).applyQuaternion(qT), latT = tv(1, 0, 0).applyQuaternion(qT);
|
|
2774
|
-
const U = tl.U;
|
|
2775
|
-
for (let i = 0; i < n; i++) tailDir(U[i], back, upT, latT, pitch[i], yaw[i]);
|
|
2776
|
-
// keep the targets off the ground
|
|
2777
|
-
const rad0 = T.radius[0] * cfg.s, rad1 = T.radius[1] * cfg.s;
|
|
2778
|
-
const p = tv();
|
|
2779
|
-
for (let it = 0; it < 3; it++) {
|
|
2780
|
-
p.copy(fr.tailBase);
|
|
2781
|
-
let worst = 0, wi = -1;
|
|
2782
|
-
for (let i = 0; i < n; i++) {
|
|
2783
|
-
p.addScaledVector(U[i], tl.len[i]);
|
|
2784
|
-
const clr = p.y - (this.terrainH(p.x, p.z) + lerp(rad0, rad1, (i + 1) / n) + 0.004 * s);
|
|
2785
|
-
if (clr < worst) { worst = clr; wi = i; }
|
|
2786
|
-
}
|
|
2787
|
-
if (wi < 0) break;
|
|
2788
|
-
const dp = Math.min(0.6, -worst / (0.3 * s));
|
|
2789
|
-
for (let i = 0; i <= wi; i++) { pitch[i] += dp * (0.4 + 0.6 * i / Math.max(1, wi)); tailDir(U[i], back, upT, latT, pitch[i], yaw[i]); }
|
|
2790
|
-
}
|
|
2791
|
-
// damped angular springs, parent-relative targets
|
|
2792
|
-
const D = tl.D, Wv = tl.W;
|
|
2793
|
-
if (!tl.init) { for (let i = 0; i < n; i++) { D[i].copy(U[i]); Wv[i].set(0, 0, 0); } tl.init = true; }
|
|
2794
|
-
const stiffen = lerp(1, 1.5, gal) * T.stiffness * P.tailStiff;
|
|
2795
|
-
const wAbs = lerp(0.3, 0.75, smooth(2, 10, vn));
|
|
2796
|
-
const sub = this.lod >= 2 ? 1 : this.lod === 1 ? Math.max(1, Math.ceil(dt / (1 / 60))) : Math.max(1, Math.ceil(dt / (1 / 120)));
|
|
2797
|
-
const hstep = dt / sub;
|
|
2798
|
-
for (let i = 1; i < n; i++) tl.rel[i].setFromUnitVectors(U[i - 1], U[i]);
|
|
2799
|
-
// follow-through: the stride carries the tail's root up and down and from side to side; the tail
|
|
2800
|
-
// lags it like a chain of pendulums: the root's acceleration (low-passed, vertical and sideways)
|
|
2801
|
-
// spins each segment by d x (-a) / its distance from the root, and the springs swing it back.
|
|
2802
|
-
// (Through the spring targets it was filtered away: the root's bounce at twice the stride frequency
|
|
2803
|
-
// is above the springs' own) cfg.tail.inertia: 1 default, 0 for a stiff stub
|
|
2804
|
-
{
|
|
2805
|
-
const rb = fr.tailBase;
|
|
2806
|
-
if (!tl.rootInit || dt <= 0) { tl.rP = rb.clone(); tl.rV = new THREE.Vector3(); tl.rA = new THREE.Vector3(); tl.rootInit = true; }
|
|
2807
|
-
else {
|
|
2808
|
-
const vNow = tv().subVectors(rb, tl.rP).multiplyScalar(1 / dt);
|
|
2809
|
-
const aNow = tv().subVectors(vNow, tl.rV).multiplyScalar(1 / dt);
|
|
2810
|
-
tl.rP.copy(rb); tl.rV.copy(vNow);
|
|
2811
|
-
// (the forward part is the body's own speeding up and braking, which the balance model and the
|
|
2812
|
-
// carriage table handle: only the vertical and sideways parts; sideways fades in turns)
|
|
2813
|
-
const F = fr.F, aF = aNow.x * F.x + aNow.z * F.z;
|
|
2814
|
-
aNow.x -= F.x * aF; aNow.z -= F.z * aF;
|
|
2815
|
-
const tw = 1 - smooth(0.2, 0.8, Math.abs(this.yawRate));
|
|
2816
|
-
aNow.x *= tw; aNow.z *= tw;
|
|
2817
|
-
aNow.clampLength(0, 14 * cfg.sk);
|
|
2818
|
-
tl.rA.lerp(aNow, 1 - Math.exp(-dt / 0.03));
|
|
2819
|
-
}
|
|
2820
|
-
// (locomotion only: a hit or a fall jolts the hips far harder than a stride, and the root
|
|
2821
|
-
// segment, which carries the skin of the rump, stays with the pelvis)
|
|
2822
|
-
const inr = (T.inertia ?? 1) * P.gaitW * P.legGait * this._bobGate * smooth(0.05, 0.4, vn);
|
|
2823
|
-
if (inr > 0) {
|
|
2824
|
-
let cum = 0;
|
|
2825
|
-
const ax = tv();
|
|
2826
|
-
for (let i = 0; i < n; i++) {
|
|
2827
|
-
cum += tl.len[i];
|
|
2828
|
-
if (i === 0) continue;
|
|
2829
|
-
ax.crossVectors(D[i], tl.rA).multiplyScalar(-inr * dt / Math.max(cum - 0.5 * tl.len[i], 0.15 * cfg.k)).clampLength(0, 2 * dt * 60);
|
|
2830
|
-
Wv[i].add(ax);
|
|
2831
|
-
}
|
|
2832
|
-
}
|
|
2833
|
-
}
|
|
2834
|
-
integrateTail(D, Wv, U, tl.rel, n, sub, hstep, stiffen, wAbs);
|
|
2835
|
-
// bones along the chain; ground contact as a hard constraint (rotate a segment up just enough)
|
|
2836
|
-
tl.p[0].copy(fr.tailBase);
|
|
2837
|
-
const latRef = tv(1, 0, 0).applyQuaternion(fr.qPel);
|
|
2838
|
-
for (let i = 0; i < n; i++) {
|
|
2839
|
-
const pi = tl.p[i], pn = tl.p[i + 1];
|
|
2840
|
-
pn.copy(pi).addScaledVector(D[i], tl.len[i]);
|
|
2841
|
-
const floor = this.terrainH(pn.x, pn.z) + lerp(rad0, rad1, (i + 1) / n);
|
|
2842
|
-
if (pn.y < floor) {
|
|
2843
|
-
const dy = clamp((floor - pi.y) / tl.len[i], -1, 1);
|
|
2844
|
-
// keep the horizontal heading of the segment, raise its slope to clear the ground
|
|
2845
|
-
const hx = D[i].x, hz = D[i].z, hl = Math.hypot(hx, hz) || 1;
|
|
2846
|
-
const c = Math.sqrt(1 - dy * dy);
|
|
2847
|
-
D[i].set(hx / hl * c, dy, hz / hl * c);
|
|
2848
|
-
Wv[i].multiplyScalar(0.5);
|
|
2849
|
-
pn.copy(pi).addScaledVector(D[i], tl.len[i]);
|
|
2850
|
-
}
|
|
2851
|
-
latRef.addScaledVector(D[i], -latRef.dot(D[i])).normalize();
|
|
2852
|
-
this.setBoneF(this.tailNames[i], pi, D[i], latRef);
|
|
2853
|
-
}
|
|
2854
|
-
}
|
|
2855
|
-
|
|
2856
|
-
// ----------------------------------------------------------------- state
|
|
2857
|
-
publish() {
|
|
2858
|
-
const st = this.state, P = this.P;
|
|
2859
|
-
st.heading = this.heading;
|
|
2860
|
-
st.speed = this.speed;
|
|
2861
|
-
st.gait = this.air.active ? 'jump' : st.stats.gait;
|
|
2862
|
-
st.grounded = !this.air.active;
|
|
2863
|
-
st.eyelid = P.eyelid;
|
|
2864
|
-
st.furRaise = P.furRaise || 0;
|
|
2865
|
-
st.action = this.actionsLayer.current();
|
|
2866
|
-
st.posture = this.actionsLayer.posture;
|
|
2867
|
-
for (let i = 0; i < 4; i++) {
|
|
2868
|
-
const leg = this.legs[i], o = st.legs[i];
|
|
2869
|
-
const pl = P.legs[i];
|
|
2870
|
-
// (a foot a posture or action has started to move is no longer a planted gait foot)
|
|
2871
|
-
o.stance = leg.state === 'stance' && !this.air.active && P.legGait > 0.5 && pl.fold + pl.sit + pl.side + pl.tuck + pl.reach < 0.005;
|
|
2872
|
-
o.contact.copy(leg.contact);
|
|
2873
|
-
}
|
|
2874
|
-
}
|
|
2875
|
-
}
|