feat(Crowd): improve 'update' method, add support for fixed time step…

…ping with interpolation

.changeset/few-pandas-occur.md

@@ -0,0 +1,74 @@
+---
+'@recast-navigation/core': minor
+'recast-navigation': minor
+---
+
+feat(Crowd): improve 'update' method, add support for fixed time stepping with interpolation
+
+The previous `update` method did a form of variable time stepping with a target time step.
+
+This has been replaced with a method that supports fixed time stepping, variable time stepping, and fixed time stepping with interpolation.
+
+The new method signature is:
+
+```ts
+update(dt: number, timeSinceLastCalled?: number, maxSubSteps?: number): void;
+```
+
+To perform a variable sized time step update, call `update` with only the `dt` parameter.
+
+```ts
+crowd.update(1 / 60);
+
+// or
+
+crowd.update(deltaTime);
+```
+
+To perform fixed time stepping with interpolation, call `update` with the `dt`, `timeSinceLastCalled`, and `maxSubSteps` parameters.
+
+```ts
+const dt = 1 / 60;
+const timeSinceLastCalled = /* get this from your game loop */;
+const maxSubSteps = 10; // optional, default is 10
+
+crowd.update(dt, timeSinceLastCalled, maxSubSteps);
+```
+
+The interpolated position of the agents can be retrieved from `agent.interpolatedPosition`.
+
+If the old behavior is desired, the following can be done:
+
+```ts
+const crowd = new Crowd(navMesh);
+
+const targetStepSize = 1 / 60;
+const maxSubSteps = 10;
+const episilon = 0.001;
+
+const update = (deltaTime: number) => {
+  if (deltaTime <= Epsilon) {
+    return;
+  }
+
+  if (targetStepSize <= episilon) {
+    crowd.update(deltaTime);
+  } else {
+    let iterationCount = Math.floor(deltaTime / targetStepSize);
+
+    if (iterationCount > maxSubSteps) {
+      iterationCount = maxSubSteps;
+    }
+    if (iterationCount < 1) {
+      iterationCount = 1;
+    }
+
+    const step = deltaTime / iterationCount;
+    for (let i = 0; i < iterationCount; i++) {
+      crowd.update(step);
+    }
+  }
+};
+```
+
+As part of this change, the `maximumSubStepCount` and `timeFactor` Crowd properties have been removed.

packages/recast-navigation-core/src/crowd.ts

@@ -3,8 +3,6 @@ import { NavMeshQuery, QueryFilter } from './nav-mesh-query';
 import { Raw, RawModule } from './raw';
 import { Vector3, vec3 } from './utils';
 
-const Epsilon = 0.001;
-
 export type CrowdAgentParams = {
   /**
    * The radius of the agent.
@@ -182,6 +180,14 @@ export class CrowdAgent implements CrowdAgentParams {
     this.raw.params.userData = value;
   }
 
+  /**
+   * The interpolated position of the agent.
+   *
+   * Use this if stepping the crowd with interpolation.
+   * This will not be updated if stepping the crowd without interpolation.
+   */
+  interpolatedPosition: Vector3 = { x: 0, y: 0, z: 0 };
+
   constructor(
     public crowd: Crowd,
     public agentIndex: number
@@ -239,6 +245,8 @@ export class CrowdAgent implements CrowdAgentParams {
       vec3.toArray(this.crowd.navMeshQuery.defaultQueryHalfExtents),
       this.crowd.navMeshQuery.defaultFilter.raw
     );
+
+    vec3.copy(position, this.interpolatedPosition);
   }
 
   /**
@@ -399,32 +407,20 @@ export class Crowd {
   navMesh: NavMesh;
 
   /**
-   * If delta time in navigation tick update is greater than the time step a number of sub iterations are done.
-   * If more iterations are need to reach deltatime they will be discarded.
-   * A value of 0 will set to no maximum and update will use as many substeps as needed.
-   */
-  maximumSubStepCount = 10;
-
-  /**
-   * Get the time step of the navigation tick update.
-   */
-  timeStep = 1 / 60;
-
-  /**
-   * Time factor applied when updating crowd agents (default 1). A value of 0 will pause crowd updates.
+   * The NavMeshQuery used to find nearest polys for commands
    */
-  timeFactor = 1;
+  navMeshQuery: NavMeshQuery;
 
   /**
-   * The NavMeshQuery used to find nearest polys for commands
+   * Accumulator for fixed updates
    */
-  navMeshQuery: NavMeshQuery;
+  private accumulator = 0;
 
   /**
-   * 
+   *
    * @param navMesh the navmesh the crowd will use for planning
    * @param param1 the crowd parameters
-   * 
+   *
    * @example
    * ```ts
    * const crowd = new Crowd(navMesh, {
@@ -444,31 +440,39 @@ export class Crowd {
   }
 
   /**
-   * Updates the crowd
-   */
-  update(deltaTime: number) {
-    if (deltaTime <= Epsilon) {
-      return;
-    }
-
-    // update crowd
-    const { timeStep } = this;
-    const maxStepCount = this.maximumSubStepCount;
-
-    if (timeStep <= Epsilon) {
-      this.raw.update(deltaTime, undefined!);
+   * Steps the crowd forward in time with a fixed time step.
+   *
+   * There are two modes. The simple mode is fixed timestepping without interpolation. In this case you only use the first argument. The second case uses interpolation. In that you also provide the time since the function was last used, as well as the maximum fixed timesteps to take.
+   *
+   * @param dt The fixed time step size to use.
+   * @param timeSinceLastCalled The time elapsed since the function was last called.
+   * @param maxSubSteps Maximum number of fixed steps to take per function call.
+   */
+  update(dt: number, timeSinceLastCalled?: number, maxSubSteps: number = 10) {
+    if (timeSinceLastCalled === undefined) {
+      // fixed step
+      this.raw.update(dt, undefined!);
     } else {
-      let iterationCount = Math.floor(deltaTime / timeStep);
-      if (maxStepCount && iterationCount > maxStepCount) {
-        iterationCount = maxStepCount;
-      }
-      if (iterationCount < 1) {
-        iterationCount = 1;
+      this.accumulator += timeSinceLastCalled;
+
+      // Do fixed steps to catch up
+      let substeps = 0;
+      while (this.accumulator >= dt && substeps < maxSubSteps) {
+        this.raw.update(dt, undefined!);
+        this.accumulator -= dt;
+        substeps++;
       }
 
-      const step = deltaTime / iterationCount;
-      for (let i = 0; i < iterationCount; i++) {
-        this.raw.update(step, undefined!);
+      // Interpolate the agent positions
+      const t = (this.accumulator % dt) / dt;
+      const agents = this.getAgents();
+      for (const agent of agents) {
+        vec3.lerp(
+          agent.interpolatedPosition,
+          agent.position(),
+          t,
+          agent.interpolatedPosition
+        );
       }
     }
   }

packages/recast-navigation-core/src/utils.ts

@@ -30,6 +30,21 @@ export const vec3 = {
   toArray: ({ x, y, z }: Vector3): Vector3Tuple => {
     return [x, y, z];
   },
+  lerp: (
+    a: Vector3,
+    b: Vector3,
+    t: number,
+    out: Vector3 = { x: 0, y: 0, z: 0 }
+  ) => {
+    out.x = a.x + (b.x - a.x) * t;
+    out.y = a.y + (b.y - a.y) * t;
+    out.z = a.z + (b.z - a.z) * t;
+  },
+  copy: (a: Vector3, out: Vector3 = { x: 0, y: 0, z: 0 }) => {
+    out.x = a.x;
+    out.y = a.y;
+    out.z = a.z;
+  },
 };
 
 export const array = <T>(getter: (index: number) => T, count: number) => {

packages/recast-navigation/.storybook/common/agent-path.tsx

@@ -1,6 +1,7 @@
 import { Line } from '@react-three/drei';
 import { CrowdAgent } from '@recast-navigation/core';
 import React, { useEffect, useState } from 'react';
+import * as THREE from 'three';
 
 export type AgentPathProps = {
   agent?: CrowdAgent;

packages/recast-navigation/.storybook/preview.ts

@@ -20,6 +20,7 @@ export const parameters = {
         'Off Mesh Connections',
         'Helpers',
         'Advanced',
+        'Debug',
       ],
       locales: 'en-US',
     },

packages/recast-navigation/.storybook/stories/crowd/crowd-with-multiple-agents.stories.tsx

@@ -0,0 +1,112 @@
+import { OrbitControls } from '@react-three/drei';
+import { ThreeEvent, useFrame } from '@react-three/fiber';
+import { Crowd, NavMesh, NavMeshQuery } from '@recast-navigation/core';
+import React, { useEffect, useState } from 'react';
+import { threeToSoloNavMesh } from 'recast-navigation/three';
+import { Group, Mesh, MeshStandardMaterial } from 'three';
+import { Debug } from '../../common/debug';
+import { NavTestEnvironment } from '../../common/nav-test-environment';
+import { decorators } from '../../decorators';
+import { parameters } from '../../parameters';
+
+export default {
+  title: 'Crowd / Crowd With Multiple Agents',
+  decorators,
+  parameters,
+};
+
+const agentMaterial = new MeshStandardMaterial({
+  color: 'red',
+});
+
+export const CrowdWithMultipleAgents = () => {
+  const [group, setGroup] = useState<Group | null>(null);
+
+  const [navMesh, setNavMesh] = useState<NavMesh | undefined>();
+  const [navMeshQuery, setNavMeshQuery] = useState<NavMeshQuery | undefined>();
+  const [crowd, setCrowd] = useState<Crowd | undefined>();
+
+  useEffect(() => {
+    if (!group) return;
+
+    const meshes: Mesh[] = [];
+
+    group.traverse((child) => {
+      if (child instanceof Mesh) {
+        meshes.push(child);
+      }
+    });
+
+    const maxAgentRadius = 0.15;
+    const cellSize = 0.05;
+
+    const { success, navMesh } = threeToSoloNavMesh(meshes, {
+      cs: cellSize,
+      ch: 0.2,
+      walkableRadius: Math.ceil(maxAgentRadius / cellSize),
+    });
+
+    if (!success) return;
+
+    const navMeshQuery = new NavMeshQuery(navMesh);
+
+    const crowd = new Crowd(navMesh, {
+      maxAgents: 10,
+      maxAgentRadius,
+    });
+
+    for (let i = 0; i < 10; i++) {
+      const { randomPoint: position } =
+        navMeshQuery.findRandomPointAroundCircle({ x: -2, y: 0, z: 3 }, 1);
+
+      crowd.addAgent(position, {
+        radius: 0.1 + Math.random() * 0.05,
+        height: 0.5,
+        maxAcceleration: 4.0,
+        maxSpeed: 1.0,
+        separationWeight: 1.0,
+      });
+    }
+
+    setNavMesh(navMesh);
+    setNavMeshQuery(navMeshQuery);
+    setCrowd(crowd);
+
+    return () => {
+      crowd.destroy();
+      navMesh.destroy();
+
+      setNavMesh(undefined);
+      setNavMeshQuery(undefined);
+      setCrowd(undefined);
+    };
+  }, [group]);
+
+  useFrame((_, delta) => {
+    if (!crowd) return;
+
+    crowd.update(delta);
+  });
+
+  const onClick = (e: ThreeEvent<MouseEvent>) => {
+    if (!navMesh || !navMeshQuery || !crowd) return;
+
+    const { point: target } = navMeshQuery.findClosestPoint(e.point);
+
+    for (const agent of crowd.getAgents()) {
+      agent.requestMoveTarget(target);
+    }
+  };
+
+  return (
+    <>
+      <group ref={setGroup} onClick={onClick}>
+        <NavTestEnvironment />
+      </group>
+
+      <Debug navMesh={navMesh} crowd={crowd} agentMaterial={agentMaterial} />
+
+      <OrbitControls />
+    </>
+  );
+};