chore(controller): three-player-controller als Ordner statt Submodul
Das Submodul zeigte auf GitHub, der eingetragene Stand 028c652 lag aber nur lokal vor: GitHub-Master ac39682 plus Showcase mit Figuren und launch.json. Frische Klone konnten den Controller deshalb nicht laden. Jetzt liegt dieser Stand als normaler Ordner im Repo, ohne Verweis auf GitHub. dist/ und node_modules/ im Controller bleiben ignoriert und werden wie bisher lokal gebaut. Co-Authored-By: Claude Opus 5 <noreply@anthropic.com>
This commit is contained in:
knsoandClaude Opus 5 committed
1 parent
780e246fe5
commit
fb38652e7f
153 files changed
+42561
-4
No files matched your search
@@ -0,0 +1,182 @@
|
||||
import type * as THREE from "three";
|
||||
import { CollisionGroup } from "./groups";
|
||||
|
||||
/** 碰撞体运动类型。 */
|
||||
export type MotionType = "static" | "kinematic" | "dynamic";
|
||||
|
||||
/**
|
||||
* 碰撞形状。
|
||||
* mesh 挂 BVH 供射线 / shapecast；box / sphere 用于动态刚体登记与解析碰撞。
|
||||
*/
|
||||
export type CollisionShape =
|
||||
| { kind: "mesh"; mesh: THREE.Mesh }
|
||||
| { kind: "box"; halfExtents: THREE.Vector3 }
|
||||
| { kind: "sphere"; radius: number };
|
||||
|
||||
/** 世界中已登记的一条碰撞体。 */
|
||||
export type CollisionCollider = {
|
||||
id: number; // 稳定标识
|
||||
motion: MotionType; // 运动类型
|
||||
shape: CollisionShape; // 形状
|
||||
groups: number; // 自己属于哪些业务组
|
||||
mask: number; // 自己要撞哪些组
|
||||
ready: boolean; // BVH / 资源是否可参与查询
|
||||
userData?: unknown; // 业务侧挂载数据
|
||||
};
|
||||
|
||||
/** 底层登记参数。未传的 groups / mask 按 motion 给默认值。 */
|
||||
export type AddColliderOptions = {
|
||||
motion: MotionType;
|
||||
shape: CollisionShape;
|
||||
groups?: number;
|
||||
mask?: number;
|
||||
ready?: boolean;
|
||||
userData?: unknown;
|
||||
};
|
||||
|
||||
/**
|
||||
* 查询过滤。
|
||||
* 传入完整 collider 时双方 groups/mask 都要匹配；只传 `{ mask }` 时单向过滤。
|
||||
*/
|
||||
export type MeshQueryFilter = CollisionCollider | { mask: number };
|
||||
|
||||
/** query / queryMeshes 的可选过滤。 */
|
||||
export type MeshQueryOptions = {
|
||||
skipIds?: number[]; // 额外排除的 id
|
||||
kinds?: CollisionShape["kind"][]; // 只保留这些形状
|
||||
motion?: MotionType | MotionType[]; // 只保留这些运动类型
|
||||
};
|
||||
|
||||
/** 按运动类型给出默认业务分组与掩码。 */
|
||||
function defaultBits(motion: MotionType): { groups: number; mask: number } {
|
||||
switch (motion) {
|
||||
case "static":
|
||||
return {
|
||||
groups: CollisionGroup.DEFAULT,
|
||||
mask: CollisionGroup.DEBRIS | CollisionGroup.VEHICLE,
|
||||
};
|
||||
case "kinematic":
|
||||
return {
|
||||
groups: CollisionGroup.DEFAULT,
|
||||
mask: CollisionGroup.DEBRIS | CollisionGroup.VEHICLE | CollisionGroup.CHARACTER,
|
||||
};
|
||||
case "dynamic":
|
||||
return {
|
||||
groups: CollisionGroup.DEBRIS,
|
||||
mask: CollisionGroup.DEFAULT | CollisionGroup.VEHICLE | CollisionGroup.DEBRIS,
|
||||
};
|
||||
}
|
||||
}
|
||||
|
||||
/** 是否为完整 collider（相对仅含 mask 的单向查询）。 */
|
||||
function isCollider(filter: MeshQueryFilter): filter is CollisionCollider {
|
||||
return "id" in filter && "motion" in filter && "groups" in filter;
|
||||
}
|
||||
|
||||
/**
|
||||
* 碰撞体注册表。
|
||||
* 人物仍用胶囊推出，车辆仍用射线 / OBB；这里只统一「谁登记了、谁和谁该测」。
|
||||
*/
|
||||
export class CollisionWorld {
|
||||
private nextId = 1; // 递增 id
|
||||
private readonly colliders = new Map<number, CollisionCollider>(); // 已登记碰撞体
|
||||
|
||||
/** 登记一条碰撞体，返回带 id 的条目。 */
|
||||
add(options: AddColliderOptions): CollisionCollider {
|
||||
const bits = defaultBits(options.motion);
|
||||
const entry: CollisionCollider = {
|
||||
id: this.nextId++,
|
||||
motion: options.motion,
|
||||
shape: options.shape,
|
||||
groups: options.groups ?? bits.groups,
|
||||
mask: options.mask ?? bits.mask,
|
||||
ready: options.ready ?? true,
|
||||
userData: options.userData,
|
||||
};
|
||||
this.colliders.set(entry.id, entry);
|
||||
return entry;
|
||||
}
|
||||
|
||||
/** 按 id 取碰撞体，不存在则返回 undefined。 */
|
||||
get(id: number): CollisionCollider | undefined {
|
||||
return this.colliders.get(id);
|
||||
}
|
||||
|
||||
/** 注销碰撞体。 */
|
||||
remove(id: number): void {
|
||||
this.colliders.delete(id);
|
||||
}
|
||||
|
||||
/** 标记 BVH / 资源是否可参与查询。 */
|
||||
setReady(id: number, ready: boolean): void {
|
||||
const entry = this.colliders.get(id);
|
||||
if (entry) entry.ready = ready;
|
||||
}
|
||||
|
||||
/** 覆盖碰撞掩码。 */
|
||||
setMask(id: number, mask: number): void {
|
||||
const entry = this.colliders.get(id);
|
||||
if (entry) entry.mask = mask;
|
||||
}
|
||||
|
||||
/** 覆盖业务分组。 */
|
||||
setGroups(id: number, groups: number): void {
|
||||
const entry = this.colliders.get(id);
|
||||
if (entry) entry.groups = groups;
|
||||
}
|
||||
|
||||
/** 覆盖 userData。 */
|
||||
setUserData(id: number, userData: unknown): void {
|
||||
const entry = this.colliders.get(id);
|
||||
if (entry) entry.userData = userData;
|
||||
}
|
||||
|
||||
/**
|
||||
* 查询可参与检测的碰撞体。
|
||||
* 完整 collider：双向 groups/mask；仅 mask：单向。自动跳过未 ready 与自身 id。
|
||||
*/
|
||||
query(filter: MeshQueryFilter, options?: MeshQueryOptions): CollisionCollider[] {
|
||||
const skip = new Set(options?.skipIds);
|
||||
const self = isCollider(filter) ? filter : null;
|
||||
if (self) skip.add(self.id);
|
||||
const mask = filter.mask;
|
||||
const kinds = options?.kinds;
|
||||
const motions = options?.motion
|
||||
? (Array.isArray(options.motion) ? options.motion : [options.motion])
|
||||
: null;
|
||||
const out: CollisionCollider[] = [];
|
||||
for (const col of this.colliders.values()) {
|
||||
if (!col.ready || skip.has(col.id)) continue;
|
||||
if (motions && !motions.includes(col.motion)) continue;
|
||||
if (kinds && !kinds.includes(col.shape.kind)) continue;
|
||||
if ((col.groups & mask) === 0) continue;
|
||||
if (self && (self.groups & col.mask) === 0) continue;
|
||||
out.push(col);
|
||||
}
|
||||
return out;
|
||||
}
|
||||
|
||||
/** 查询可做 BVH 射线 / shapecast 的网格。 */
|
||||
queryMeshes(filter: MeshQueryFilter, options?: MeshQueryOptions): THREE.Mesh[] {
|
||||
const meshes: THREE.Mesh[] = [];
|
||||
for (const col of this.query(filter, options)) {
|
||||
if (col.shape.kind === "mesh") meshes.push(col.shape.mesh);
|
||||
}
|
||||
return meshes;
|
||||
}
|
||||
|
||||
/** 按运动类型列出 id（供 clearColliders）。 */
|
||||
idsByMotion(motion?: MotionType): number[] {
|
||||
const out: number[] = [];
|
||||
for (const col of this.colliders.values()) {
|
||||
if (motion && col.motion !== motion) continue;
|
||||
out.push(col.id);
|
||||
}
|
||||
return out;
|
||||
}
|
||||
|
||||
/** 清空全部登记。 */
|
||||
clear(): void {
|
||||
this.colliders.clear();
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,187 @@
|
||||
import * as THREE from "three";
|
||||
import { integrateQuaternion } from "../utils/vehiclePhysics/vehicleMath";
|
||||
|
||||
/** 冲量唤醒：质心线速度增量 ‖Δv‖ = ‖J‖·invMass 超过该值则醒（世界单位/秒）。 */
|
||||
const WAKE_DELTA_V = 1e-4;
|
||||
const WAKE_DELTA_V_SQ = WAKE_DELTA_V * WAKE_DELTA_V;
|
||||
|
||||
/** 动态刚体形状判别。 */
|
||||
export type DynamicBodyKind = "sphere" | "box";
|
||||
|
||||
/** 动态刚体材质 / 初态可选字段（各形状 options 共用）。 */
|
||||
export type DynamicBodyMaterialOptions = {
|
||||
/** 密度，质量 = 体积 × 密度；默认见 DYNAMIC_BODY_DEFAULTS。 */
|
||||
density?: number;
|
||||
/** 弹性，0 不反弹，1 完全弹；默认见 DYNAMIC_BODY_DEFAULTS。 */
|
||||
restitution?: number;
|
||||
/** 摩擦；默认见 DYNAMIC_BODY_DEFAULTS。 */
|
||||
friction?: number;
|
||||
/** 线阻尼，越大滑行越快停；默认见 DYNAMIC_BODY_DEFAULTS。 */
|
||||
linearDamping?: number;
|
||||
/** 角阻尼，越大旋转越快停；默认见 DYNAMIC_BODY_DEFAULTS。 */
|
||||
angularDamping?: number;
|
||||
/** 重力，默认沿用控制器重力。 */
|
||||
gravity?: number;
|
||||
/** 初速度。 */
|
||||
velocity?: THREE.Vector3;
|
||||
/** 初角速度。 */
|
||||
angularVelocity?: THREE.Vector3;
|
||||
/** 视觉网格。 */
|
||||
mesh?: THREE.Mesh;
|
||||
};
|
||||
|
||||
/** 子类构造时传入的已解析材质与视觉（缺省已在上层填好）。 */
|
||||
export type DynamicBodyBaseInit = {
|
||||
density: number; // 密度
|
||||
mass: number; // 质量（体积 × 密度）
|
||||
restitution: number; // 弹性
|
||||
gravity: number; // 重力加速度
|
||||
position: THREE.Vector3; // 世界位置
|
||||
velocity: THREE.Vector3; // 线速度
|
||||
angularVelocity?: THREE.Vector3; // 角速度
|
||||
linearDamping: number; // 线阻尼
|
||||
angularDamping: number; // 角阻尼
|
||||
friction: number; // 摩擦
|
||||
mesh: THREE.Mesh; // 视觉网格
|
||||
debugMesh: THREE.Mesh; // 碰撞线框
|
||||
};
|
||||
|
||||
/**
|
||||
* 动态刚体基类（ImpulseBody + 材质）。
|
||||
* 形状相关惯量 / 特征尺寸由子类提供；窄相算法按 kind 分派。
|
||||
*/
|
||||
export abstract class DynamicBody {
|
||||
abstract readonly kind: DynamicBodyKind; // 形状判别
|
||||
|
||||
density: number; // 密度
|
||||
/** 由 density × 体积得到，供冲量求解使用。 */
|
||||
mass: number;
|
||||
restitution: number; // 弹性 [0, 1]
|
||||
gravity: number; // 重力加速度（通常为负）
|
||||
position = new THREE.Vector3(); // 世界位置
|
||||
quaternion = new THREE.Quaternion(); // 世界朝向
|
||||
/** 线速度（保留 velocity 命名以兼容现有调用）。 */
|
||||
velocity = new THREE.Vector3();
|
||||
angularVelocity = new THREE.Vector3(); // 角速度
|
||||
invMass = 0; // 质量倒数
|
||||
invInertia = new THREE.Vector3(); // 局部惯量倒数对角元
|
||||
linearDamping: number; // 线阻尼
|
||||
angularDamping: number; // 角阻尼
|
||||
friction: number; // 碰撞摩擦
|
||||
mesh: THREE.Mesh; // 视觉网格
|
||||
/** 碰撞形状线框，由 setDynamicBodyDebug 显隐。 */
|
||||
debugMesh: THREE.Mesh;
|
||||
contactMesh: THREE.Mesh | null = null; // 本帧接触的网格（平台携带用）
|
||||
colliderId = 0; // CollisionWorld 登记 id
|
||||
/** 休眠中则跳过积分与窄相。 */
|
||||
sleeping = false;
|
||||
/** 是否允许进入休眠。 */
|
||||
canSleep = true;
|
||||
/** 连续低于休眠阈值的累计时间（秒）。 */
|
||||
sleepTimer = 0;
|
||||
|
||||
private _r = new THREE.Vector3();
|
||||
private _torque = new THREE.Vector3();
|
||||
private _local = new THREE.Vector3();
|
||||
private _world = new THREE.Vector3();
|
||||
private _invQuat = new THREE.Quaternion();
|
||||
|
||||
/** 写入材质、位姿与视觉引用，并计算 invMass。 */
|
||||
protected constructor(opts: DynamicBodyBaseInit) {
|
||||
this.density = Math.max(1e-8, opts.density);
|
||||
this.mass = Math.max(1e-6, opts.mass);
|
||||
this.restitution = Math.max(0, Math.min(1, opts.restitution));
|
||||
this.gravity = opts.gravity;
|
||||
this.position.copy(opts.position);
|
||||
this.velocity.copy(opts.velocity);
|
||||
if (opts.angularVelocity) this.angularVelocity.copy(opts.angularVelocity);
|
||||
this.linearDamping = opts.linearDamping;
|
||||
this.angularDamping = opts.angularDamping;
|
||||
this.friction = opts.friction;
|
||||
this.mesh = opts.mesh;
|
||||
this.debugMesh = opts.debugMesh;
|
||||
this.invMass = 1 / this.mass;
|
||||
}
|
||||
|
||||
/** 用于速度钳制 / 携带阈值的特征半尺寸。 */
|
||||
abstract characteristicExtent(): number;
|
||||
|
||||
/** 唤醒刚体。 */
|
||||
wakeUp(): void {
|
||||
if (!this.sleeping) return;
|
||||
this.sleeping = false;
|
||||
this.sleepTimer = 0;
|
||||
}
|
||||
|
||||
/** 进入休眠并清零速度。 */
|
||||
putToSleep(): void {
|
||||
if (!this.canSleep) return;
|
||||
this.sleeping = true;
|
||||
this.sleepTimer = 0;
|
||||
this.velocity.set(0, 0, 0);
|
||||
this.angularVelocity.set(0, 0, 0);
|
||||
this.contactMesh = null;
|
||||
}
|
||||
|
||||
/** 线速度 / 角速度是否低于给定阈值。 */
|
||||
isRestingCandidate(linearThreshold: number, angularThreshold: number): boolean {
|
||||
return (
|
||||
this.velocity.lengthSq() <= linearThreshold * linearThreshold
|
||||
&& this.angularVelocity.lengthSq() <= angularThreshold * angularThreshold
|
||||
);
|
||||
}
|
||||
|
||||
/** 读取世界点处的刚体速度：v + ω × r。 */
|
||||
getVelocityAtPoint(point: THREE.Vector3, out: THREE.Vector3): THREE.Vector3 {
|
||||
this._r.subVectors(point, this.position);
|
||||
return out.copy(this.velocity).add(this._world.copy(this.angularVelocity).cross(this._r));
|
||||
}
|
||||
|
||||
/**
|
||||
* 休眠唤醒：速度增量 ‖J‖·invMass 超过阈值则醒。
|
||||
*/
|
||||
private wakeIfImpulseMeaningful(impulse: THREE.Vector3): void {
|
||||
if (!this.sleeping) return;
|
||||
const inv = this.invMass;
|
||||
if (impulse.lengthSq() * inv * inv > WAKE_DELTA_V_SQ) this.wakeUp();
|
||||
}
|
||||
|
||||
/** 在质心施加冲量。 */
|
||||
applyImpulse(impulse: THREE.Vector3): void {
|
||||
this.wakeIfImpulseMeaningful(impulse);
|
||||
this.velocity.addScaledVector(impulse, this.invMass);
|
||||
}
|
||||
|
||||
/** 在世界点施加冲量，同时产生角速度。 */
|
||||
applyImpulseAtPoint(impulse: THREE.Vector3, contactPoint: THREE.Vector3): void {
|
||||
this.wakeIfImpulseMeaningful(impulse);
|
||||
this.velocity.addScaledVector(impulse, this.invMass);
|
||||
this._r.subVectors(contactPoint, this.position);
|
||||
this.angularVelocity.add(this.angularDeltaFromImpulse(impulse, this._r));
|
||||
}
|
||||
|
||||
/** 将冲量转为世界角速度增量：ω += R I⁻¹ Rᵀ (r × J)。 */
|
||||
private angularDeltaFromImpulse(impulse: THREE.Vector3, r: THREE.Vector3): THREE.Vector3 {
|
||||
this._torque.copy(r).cross(impulse);
|
||||
this._invQuat.copy(this.quaternion).invert();
|
||||
this._local.copy(this._torque).applyQuaternion(this._invQuat);
|
||||
this._local.x *= this.invInertia.x;
|
||||
this._local.y *= this.invInertia.y;
|
||||
this._local.z *= this.invInertia.z;
|
||||
return this._world.copy(this._local).applyQuaternion(this.quaternion);
|
||||
}
|
||||
|
||||
/** 指数线 / 角阻尼。 */
|
||||
applyDamping(dt: number): void {
|
||||
const lin = Math.exp(-this.linearDamping * dt);
|
||||
const ang = Math.exp(-this.angularDamping * dt);
|
||||
this.velocity.multiplyScalar(lin);
|
||||
this.angularVelocity.multiplyScalar(ang);
|
||||
}
|
||||
|
||||
/** 用当前速度积分位置和朝向。 */
|
||||
integrate(dt: number): void {
|
||||
this.position.addScaledVector(this.velocity, dt);
|
||||
integrateQuaternion(this.quaternion, this.angularVelocity, dt);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,171 @@
|
||||
import * as THREE from "three";
|
||||
import { DynamicBody } from "./DynamicBody";
|
||||
import type { CollisionTemps } from "../utils/capsuleCollision";
|
||||
|
||||
/**
|
||||
* 动态盒刚体。
|
||||
* 惯量按实心长方体；与网格的接触冲量走 ContactImpulseSolver，盒–盒走 resolveObbObb。
|
||||
*/
|
||||
export class DynamicBoxBody extends DynamicBody {
|
||||
readonly kind = "box" as const; // 形状判别
|
||||
halfExtents = new THREE.Vector3(); // 碰撞盒半边长
|
||||
|
||||
/** 按体积 × 密度算质量，并写入长方体惯量。 */
|
||||
constructor(opts: {
|
||||
halfExtents: THREE.Vector3;
|
||||
density: number;
|
||||
restitution: number;
|
||||
gravity: number;
|
||||
position: THREE.Vector3;
|
||||
quaternion?: THREE.Quaternion;
|
||||
velocity: THREE.Vector3;
|
||||
angularVelocity?: THREE.Vector3;
|
||||
linearDamping: number;
|
||||
angularDamping: number;
|
||||
friction: number;
|
||||
mesh: THREE.Mesh;
|
||||
debugMesh: THREE.Mesh;
|
||||
}) {
|
||||
const hx = Math.max(1e-4, opts.halfExtents.x);
|
||||
const hy = Math.max(1e-4, opts.halfExtents.y);
|
||||
const hz = Math.max(1e-4, opts.halfExtents.z);
|
||||
const volume = 8 * hx * hy * hz;
|
||||
super({
|
||||
density: opts.density,
|
||||
mass: volume * Math.max(1e-8, opts.density),
|
||||
restitution: opts.restitution,
|
||||
gravity: opts.gravity,
|
||||
position: opts.position,
|
||||
velocity: opts.velocity,
|
||||
angularVelocity: opts.angularVelocity,
|
||||
linearDamping: opts.linearDamping,
|
||||
angularDamping: opts.angularDamping,
|
||||
friction: opts.friction,
|
||||
mesh: opts.mesh,
|
||||
debugMesh: opts.debugMesh,
|
||||
});
|
||||
this.halfExtents.set(hx, hy, hz);
|
||||
if (opts.quaternion) this.quaternion.copy(opts.quaternion);
|
||||
this.setCuboidInertia();
|
||||
}
|
||||
|
||||
/** 特征半尺寸取三边最大值。 */
|
||||
characteristicExtent(): number {
|
||||
return Math.max(this.halfExtents.x, this.halfExtents.y, this.halfExtents.z);
|
||||
}
|
||||
|
||||
/** 实心长方体惯量 I = m/12 (h²+d²) 等，并写 invInertia。 */
|
||||
setCuboidInertia(): void {
|
||||
const m = this.mass;
|
||||
const w = this.halfExtents.x * 2;
|
||||
const h = this.halfExtents.y * 2;
|
||||
const d = this.halfExtents.z * 2;
|
||||
const ix = Math.max(1e-12, m / 12 * (h * h + d * d));
|
||||
const iy = Math.max(1e-12, m / 12 * (w * w + d * d));
|
||||
const iz = Math.max(1e-12, m / 12 * (w * w + h * h));
|
||||
this.invInertia.set(1 / ix, 1 / iy, 1 / iz);
|
||||
}
|
||||
}
|
||||
|
||||
/** 类型收窄：动态盒。 */
|
||||
export function isBoxBody(body: DynamicBody): body is DynamicBoxBody {
|
||||
return body.kind === "box";
|
||||
}
|
||||
|
||||
const CAPSULE_OBB_SAMPLES = 8; // 胶囊轴采样点数（含端点）
|
||||
|
||||
const _invQuat = new THREE.Quaternion();
|
||||
const _local = new THREE.Vector3();
|
||||
const _sample = new THREE.Vector3();
|
||||
const _bestOnSeg = new THREE.Vector3();
|
||||
const _bestDirLocal = new THREE.Vector3();
|
||||
const _candDir = new THREE.Vector3();
|
||||
|
||||
/**
|
||||
* 胶囊 vs OBB：沿胶囊段采样，取到 AABB 的最近距离。
|
||||
* 命中时 temps.closestTri 为**胶囊指向刚体**的单位法线（与 capsuleSphereOverlap 一致），
|
||||
* temps.closestSeg 为段上点，返回穿透深度。
|
||||
*/
|
||||
export function capsuleObbOverlap(
|
||||
capsule: THREE.Object3D,
|
||||
capsuleInfo: { segment: THREE.Line3; radius: number; dynamicsSegment?: THREE.Line3 },
|
||||
boxPos: THREE.Vector3,
|
||||
boxQuat: THREE.Quaternion,
|
||||
half: THREE.Vector3,
|
||||
temps: CollisionTemps,
|
||||
): number {
|
||||
const segment = capsuleInfo.dynamicsSegment ?? capsuleInfo.segment;
|
||||
temps.localSeg.start.copy(segment.start).applyMatrix4(capsule.matrixWorld);
|
||||
temps.localSeg.end.copy(segment.end).applyMatrix4(capsule.matrixWorld);
|
||||
|
||||
_invQuat.copy(boxQuat).invert();
|
||||
const hx = half.x;
|
||||
const hy = half.y;
|
||||
const hz = half.z;
|
||||
let bestDist = Infinity;
|
||||
let insideAxis = -1; // 0/1/2 表示在盒内时选的推出轴
|
||||
let insideSign = 1;
|
||||
let insideDepth = 0;
|
||||
|
||||
for (let i = 0; i <= CAPSULE_OBB_SAMPLES; i++) {
|
||||
const t = i / CAPSULE_OBB_SAMPLES;
|
||||
_sample.lerpVectors(temps.localSeg.start, temps.localSeg.end, t);
|
||||
_local.copy(_sample).sub(boxPos).applyQuaternion(_invQuat);
|
||||
|
||||
const inside =
|
||||
Math.abs(_local.x) <= hx &&
|
||||
Math.abs(_local.y) <= hy &&
|
||||
Math.abs(_local.z) <= hz;
|
||||
|
||||
if (inside) {
|
||||
const dx = hx - Math.abs(_local.x);
|
||||
const dy = hy - Math.abs(_local.y);
|
||||
const dz = hz - Math.abs(_local.z);
|
||||
const depth = capsuleInfo.radius + Math.min(dx, dy, dz);
|
||||
if (depth > insideDepth) {
|
||||
insideDepth = depth;
|
||||
_bestOnSeg.copy(_sample);
|
||||
// 取穿透最小的面，法线先取盒心→外侧（稍后翻成胶囊→盒）
|
||||
if (dx <= dy && dx <= dz) {
|
||||
insideAxis = 0;
|
||||
insideSign = Math.sign(_local.x || 1);
|
||||
} else if (dy <= dz) {
|
||||
insideAxis = 1;
|
||||
insideSign = Math.sign(_local.y || 1);
|
||||
} else {
|
||||
insideAxis = 2;
|
||||
insideSign = Math.sign(_local.z || 1);
|
||||
}
|
||||
}
|
||||
continue;
|
||||
}
|
||||
|
||||
const cx = Math.min(hx, Math.max(-hx, _local.x));
|
||||
const cy = Math.min(hy, Math.max(-hy, _local.y));
|
||||
const cz = Math.min(hz, Math.max(-hz, _local.z));
|
||||
// 盒表面 → 采样点（盒→胶囊）；返回前取反
|
||||
_candDir.set(_local.x - cx, _local.y - cy, _local.z - cz);
|
||||
const dist = _candDir.length();
|
||||
if (dist >= bestDist) continue;
|
||||
bestDist = dist;
|
||||
_bestOnSeg.copy(_sample);
|
||||
if (dist > 1e-8) _bestDirLocal.copy(_candDir).multiplyScalar(1 / dist);
|
||||
else _bestDirLocal.set(0, 1, 0);
|
||||
}
|
||||
|
||||
if (insideDepth > 0) {
|
||||
temps.closestSeg.copy(_bestOnSeg);
|
||||
if (insideAxis === 0) temps.closestTri.set(insideSign, 0, 0);
|
||||
else if (insideAxis === 1) temps.closestTri.set(0, insideSign, 0);
|
||||
else temps.closestTri.set(0, 0, insideSign);
|
||||
// 盒外法线 → 翻成胶囊指向刚体（进入盒的方向）
|
||||
temps.closestTri.applyQuaternion(boxQuat).negate();
|
||||
return insideDepth;
|
||||
}
|
||||
|
||||
if (bestDist >= capsuleInfo.radius) return 0;
|
||||
temps.closestSeg.copy(_bestOnSeg);
|
||||
// 盒→胶囊 取反 → 胶囊→盒
|
||||
temps.closestTri.copy(_bestDirLocal).applyQuaternion(boxQuat).negate();
|
||||
return capsuleInfo.radius - bestDist;
|
||||
}
|
||||
@@ -0,0 +1,247 @@
|
||||
import * as THREE from "three";
|
||||
import { contactSkinForExtent, type RawContact } from "./contacts/ContactPoint";
|
||||
import { DynamicBody } from "./DynamicBody";
|
||||
|
||||
/**
|
||||
* 动态球刚体。
|
||||
* 球惯量 I = ⅖ m r²；与网格的接触冲量走 ContactImpulseSolver。
|
||||
*/
|
||||
export class DynamicSphereBody extends DynamicBody {
|
||||
readonly kind = "sphere" as const; // 形状判别
|
||||
radius: number; // 碰撞半径
|
||||
|
||||
/** 按体积 × 密度算质量，并写入球惯量。 */
|
||||
constructor(opts: {
|
||||
radius: number;
|
||||
density: number;
|
||||
restitution: number;
|
||||
gravity: number;
|
||||
position: THREE.Vector3;
|
||||
velocity: THREE.Vector3;
|
||||
angularVelocity?: THREE.Vector3;
|
||||
linearDamping: number;
|
||||
angularDamping: number;
|
||||
friction: number;
|
||||
mesh: THREE.Mesh;
|
||||
debugMesh: THREE.Mesh;
|
||||
}) {
|
||||
const volume = (4 / 3) * Math.PI * opts.radius * opts.radius * opts.radius;
|
||||
super({
|
||||
density: opts.density,
|
||||
mass: volume * Math.max(1e-8, opts.density),
|
||||
restitution: opts.restitution,
|
||||
gravity: opts.gravity,
|
||||
position: opts.position,
|
||||
velocity: opts.velocity,
|
||||
angularVelocity: opts.angularVelocity,
|
||||
linearDamping: opts.linearDamping,
|
||||
angularDamping: opts.angularDamping,
|
||||
friction: opts.friction,
|
||||
mesh: opts.mesh,
|
||||
debugMesh: opts.debugMesh,
|
||||
});
|
||||
this.radius = opts.radius;
|
||||
this.setSphereInertia();
|
||||
}
|
||||
|
||||
/** 特征半尺寸即半径。 */
|
||||
characteristicExtent(): number {
|
||||
return this.radius;
|
||||
}
|
||||
|
||||
/** 实心球惯量 I = ⅖ m r²（对角），并写 invInertia。 */
|
||||
setSphereInertia(): void {
|
||||
// 小尺度物体 I 可低于旧地板 1e-8；置 0 会导致无法滚动、地面摩擦过粘
|
||||
const i = Math.max(1e-12, 0.4 * this.mass * this.radius * this.radius);
|
||||
const inv = 1 / i;
|
||||
this.invInertia.set(inv, inv, inv);
|
||||
}
|
||||
}
|
||||
|
||||
/** 类型收窄：动态球。 */
|
||||
export function isSphereBody(body: DynamicBody): body is DynamicSphereBody {
|
||||
return body.kind === "sphere";
|
||||
}
|
||||
|
||||
/** 球窄相预分配临时量（避免每帧 new）。 */
|
||||
export type SphereCollisionTemps = {
|
||||
invMat: THREE.Matrix4; // mesh.matrixWorld 的逆
|
||||
normalMat: THREE.Matrix3; // 法线变换矩阵
|
||||
invQuat: THREE.Quaternion; // 盒朝向的逆
|
||||
localSphere: THREE.Sphere; // 网格本地空间中的查询球
|
||||
closest: THREE.Vector3; // 三角面 / 盒上最近点（本地）
|
||||
worldClosest: THREE.Vector3; // 最近点世界坐标
|
||||
offset: THREE.Vector3; // 球心到最近点的偏移
|
||||
before: THREE.Vector3; // 推出前位置备份
|
||||
normal: THREE.Vector3; // 接触法线（由对面指向球）
|
||||
local: THREE.Vector3; // 盒局部坐标
|
||||
contact: THREE.Vector3; // 世界接触点
|
||||
boxVel: THREE.Vector3; // 盒上接触点速度
|
||||
impulse: THREE.Vector3; // 冲量缓冲
|
||||
};
|
||||
|
||||
/** 创建球碰撞临时量。 */
|
||||
export function createSphereCollisionTemps(): SphereCollisionTemps {
|
||||
return {
|
||||
invMat: new THREE.Matrix4(),
|
||||
normalMat: new THREE.Matrix3(),
|
||||
invQuat: new THREE.Quaternion(),
|
||||
localSphere: new THREE.Sphere(),
|
||||
closest: new THREE.Vector3(),
|
||||
worldClosest: new THREE.Vector3(),
|
||||
offset: new THREE.Vector3(),
|
||||
before: new THREE.Vector3(),
|
||||
normal: new THREE.Vector3(),
|
||||
local: new THREE.Vector3(),
|
||||
contact: new THREE.Vector3(),
|
||||
boxVel: new THREE.Vector3(),
|
||||
impulse: new THREE.Vector3(),
|
||||
};
|
||||
}
|
||||
|
||||
/**
|
||||
* 收集球 vs 单个 BVH 网格的原始接触（不移动球心）。
|
||||
* 法线由表面指向球心；penetration = radius - 距离；返回写入后的 rawCount。
|
||||
*/
|
||||
export function collectSphereVsMeshContacts(
|
||||
position: THREE.Vector3,
|
||||
radius: number,
|
||||
mesh: THREE.Mesh,
|
||||
temps: SphereCollisionTemps,
|
||||
raw: RawContact[],
|
||||
rawCount: number,
|
||||
maxRaw: number,
|
||||
): number {
|
||||
const tree = (mesh.geometry as any)?.boundsTree;
|
||||
if (!tree || rawCount >= maxRaw) return rawCount;
|
||||
|
||||
// 球变到网格本地，均匀缩放用 scale.x 近似
|
||||
temps.invMat.copy(mesh.matrixWorld).invert();
|
||||
temps.localSphere.center.copy(position).applyMatrix4(temps.invMat);
|
||||
const scale = mesh.scale.x || 1;
|
||||
temps.localSphere.radius = radius / scale;
|
||||
|
||||
temps.normalMat.getNormalMatrix(mesh.matrixWorld);
|
||||
|
||||
tree.shapecast({
|
||||
intersectsBounds: (box: THREE.Box3) => box.intersectsSphere(temps.localSphere),
|
||||
intersectsTriangle: (tri: any) => {
|
||||
if (rawCount >= maxRaw) return true; // 触达上限则提前结束遍历
|
||||
tri.closestPointToPoint(temps.localSphere.center, temps.closest);
|
||||
temps.offset.subVectors(temps.localSphere.center, temps.closest);
|
||||
const dist = temps.offset.length();
|
||||
const penetration = temps.localSphere.radius - dist;
|
||||
// 允许轻微皮肤重叠（按球半径缩放，本地空间与 mesh.scale 对齐）
|
||||
const localSkin = contactSkinForExtent(radius) / Math.max(scale, 1e-8);
|
||||
if (penetration < -localSkin) return;
|
||||
|
||||
if (dist > 1e-8) temps.normal.copy(temps.offset).multiplyScalar(1 / dist);
|
||||
else temps.normal.set(0, 1, 0); // 球心几乎落在面上时给默认上法线
|
||||
temps.normal.applyMatrix3(temps.normalMat).normalize();
|
||||
|
||||
temps.worldClosest.copy(temps.closest).applyMatrix4(mesh.matrixWorld);
|
||||
const slot = raw[rawCount++];
|
||||
slot.point.copy(temps.worldClosest);
|
||||
slot.normal.copy(temps.normal);
|
||||
slot.penetration = penetration * scale; // 穿透深度还原到世界尺度
|
||||
},
|
||||
boundsTraverseOrder: (box: THREE.Box3) => box.distanceToPoint(temps.localSphere.center) - temps.localSphere.radius,
|
||||
});
|
||||
|
||||
return rawCount;
|
||||
}
|
||||
|
||||
/**
|
||||
* 球 vs OBB：重叠则推出球并写 temps.normal / contact。
|
||||
* 法线由盒指向球；会就地修改 position。
|
||||
*/
|
||||
export function collideSphereVsObb(
|
||||
position: THREE.Vector3,
|
||||
radius: number,
|
||||
boxPos: THREE.Vector3,
|
||||
boxQuat: THREE.Quaternion,
|
||||
half: THREE.Vector3,
|
||||
temps: SphereCollisionTemps,
|
||||
): boolean {
|
||||
// 球心变到盒局部
|
||||
temps.invQuat.copy(boxQuat).invert();
|
||||
temps.local.copy(position).sub(boxPos).applyQuaternion(temps.invQuat);
|
||||
|
||||
const hx = half.x;
|
||||
const hy = half.y;
|
||||
const hz = half.z;
|
||||
const inside =
|
||||
Math.abs(temps.local.x) <= hx &&
|
||||
Math.abs(temps.local.y) <= hy &&
|
||||
Math.abs(temps.local.z) <= hz;
|
||||
|
||||
if (inside) {
|
||||
// 球心已在盒内：沿穿透最小的轴推出到表面外 radius
|
||||
const dx = hx - Math.abs(temps.local.x);
|
||||
const dy = hy - Math.abs(temps.local.y);
|
||||
const dz = hz - Math.abs(temps.local.z);
|
||||
if (dx <= dy && dx <= dz) {
|
||||
temps.local.x = Math.sign(temps.local.x || 1) * (hx + radius);
|
||||
temps.normal.set(Math.sign(temps.local.x), 0, 0);
|
||||
} else if (dy <= dz) {
|
||||
temps.local.y = Math.sign(temps.local.y || 1) * (hy + radius);
|
||||
temps.normal.set(0, Math.sign(temps.local.y), 0);
|
||||
} else {
|
||||
temps.local.z = Math.sign(temps.local.z || 1) * (hz + radius);
|
||||
temps.normal.set(0, 0, Math.sign(temps.local.z));
|
||||
}
|
||||
temps.normal.applyQuaternion(boxQuat);
|
||||
position.copy(temps.local).applyQuaternion(boxQuat).add(boxPos);
|
||||
temps.contact.copy(position).addScaledVector(temps.normal, -radius);
|
||||
return true;
|
||||
}
|
||||
|
||||
// 盒外：钳到盒表面最近点，若距球心 < radius 则推出
|
||||
temps.closest.set(
|
||||
Math.min(hx, Math.max(-hx, temps.local.x)),
|
||||
Math.min(hy, Math.max(-hy, temps.local.y)),
|
||||
Math.min(hz, Math.max(-hz, temps.local.z)),
|
||||
);
|
||||
temps.offset.subVectors(temps.local, temps.closest);
|
||||
const dist = temps.offset.length();
|
||||
if (dist >= radius || dist < 1e-8) return false;
|
||||
temps.offset.multiplyScalar(1 / dist);
|
||||
temps.local.addScaledVector(temps.offset, radius - dist);
|
||||
temps.normal.copy(temps.offset).applyQuaternion(boxQuat);
|
||||
position.copy(temps.local).applyQuaternion(boxQuat).add(boxPos);
|
||||
temps.contact.copy(temps.closest).applyQuaternion(boxQuat).add(boxPos);
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* 两球按质量推出并交换法向速度。
|
||||
* 暂不走双刚体 ContactImpulseSolver（无摩擦 / 无角冲量）。
|
||||
*/
|
||||
export function resolveSphereSphere(
|
||||
a: DynamicSphereBody,
|
||||
b: DynamicSphereBody,
|
||||
temps: SphereCollisionTemps,
|
||||
restitution: number,
|
||||
): void {
|
||||
temps.offset.subVectors(a.position, b.position);
|
||||
const dist = temps.offset.length();
|
||||
const minDist = a.radius + b.radius;
|
||||
if (dist >= minDist) return;
|
||||
if (dist > 1e-8) temps.normal.copy(temps.offset).multiplyScalar(1 / dist);
|
||||
else temps.normal.set(0, 1, 0); // 完全重合时给默认法线
|
||||
|
||||
// 位置分离（按 invMass 分配）
|
||||
const depth = minDist - dist;
|
||||
const invA = a.invMass;
|
||||
const invB = b.invMass;
|
||||
const invSum = invA + invB;
|
||||
a.position.addScaledVector(temps.normal, depth * (invA / invSum));
|
||||
b.position.addScaledVector(temps.normal, -depth * (invB / invSum));
|
||||
|
||||
// 仅处理接近中的法向相对速度
|
||||
const rel = a.velocity.dot(temps.normal) - b.velocity.dot(temps.normal);
|
||||
if (rel >= 0) return;
|
||||
const impulse = -(1 + restitution) * rel / invSum;
|
||||
a.velocity.addScaledVector(temps.normal, impulse * invA);
|
||||
b.velocity.addScaledVector(temps.normal, -impulse * invB);
|
||||
}
|
||||
@@ -0,0 +1,242 @@
|
||||
import * as THREE from "three";
|
||||
import { MeshBVH, acceleratedRaycast } from "three-mesh-bvh";
|
||||
import * as BufferGeometryUtils from "three/examples/jsm/utils/BufferGeometryUtils.js";
|
||||
import type { BvhWorkerPool } from "../utils/BvhWorkerPool";
|
||||
|
||||
/**
|
||||
* 碰撞网格构建工具。
|
||||
*/
|
||||
|
||||
/** 调试辅助物及其子级不应被烘焙进 mesh collider。 */
|
||||
function isExcludedFromCollider(object: THREE.Object3D): boolean {
|
||||
let current: THREE.Object3D | null = object;
|
||||
while (current) {
|
||||
if (current.userData.excludeFromCollider === true) return true;
|
||||
current = current.parent;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
/** 校验并补全合并所需属性（position / normal / uv）；含 NaN 或顶点数不足时返回 null。 */
|
||||
export function ensureAttributesMinimal(geom: THREE.BufferGeometry): THREE.BufferGeometry | null {
|
||||
if (!geom.attributes.position) return null;
|
||||
const position = geom.attributes.position.array;
|
||||
for (let i = 0; i < position.length; i++) {
|
||||
if (!Number.isFinite(position[i])) return null;
|
||||
}
|
||||
if (geom.attributes.position.count < 3) return null;
|
||||
if (!geom.attributes.normal) geom.computeVertexNormals();
|
||||
if (!geom.attributes.uv) {
|
||||
const count = geom.attributes.position.count;
|
||||
geom.setAttribute("uv", new THREE.BufferAttribute(new Float32Array(count * 2), 2));
|
||||
}
|
||||
return geom;
|
||||
}
|
||||
|
||||
/**
|
||||
* 统一一组几何的属性布局，便于 BufferGeometryUtils.mergeGeometries。
|
||||
* 去掉非必需属性；冲突项删除；缺失项用零填充。
|
||||
*/
|
||||
export function unifiedAttribute(collected: THREE.BufferGeometry[]): THREE.BufferGeometry[] {
|
||||
type AttrMeta = { itemSize: number; arrayCtor: any; examples: number; normalized: boolean };
|
||||
const attrMap = new Map<string, AttrMeta>();
|
||||
const attrConflict = new Set<string>();
|
||||
const required = new Set(["position", "normal", "uv"]);
|
||||
|
||||
for (const g of collected)
|
||||
for (const name of Object.keys(g.attributes))
|
||||
if (!required.has(name)) g.deleteAttribute(name);
|
||||
|
||||
for (const g of collected) {
|
||||
for (const name of Object.keys(g.attributes)) {
|
||||
const attr = g.attributes[name] as THREE.BufferAttribute;
|
||||
const ctor = (attr.array as any).constructor;
|
||||
if (!attrMap.has(name)) {
|
||||
attrMap.set(name, { itemSize: attr.itemSize, arrayCtor: ctor, examples: 1, normalized: attr.normalized });
|
||||
} else {
|
||||
const m = attrMap.get(name)!;
|
||||
if (m.itemSize !== attr.itemSize || m.arrayCtor !== ctor || m.normalized !== attr.normalized) attrConflict.add(name);
|
||||
else m.examples++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
for (const name of attrConflict) {
|
||||
for (const g of collected) if (g.attributes[name]) g.deleteAttribute(name);
|
||||
attrMap.delete(name);
|
||||
}
|
||||
|
||||
for (const [name, meta] of attrMap) {
|
||||
for (const g of collected) {
|
||||
if (!g.attributes[name]) {
|
||||
const count = g.attributes.position.count;
|
||||
g.setAttribute(name, new THREE.BufferAttribute(new meta.arrayCtor(count * meta.itemSize), meta.itemSize, meta.normalized));
|
||||
}
|
||||
}
|
||||
}
|
||||
return collected;
|
||||
}
|
||||
|
||||
/** 遍历 sources，把 Mesh / LineSegments 克隆到世界空间并收集（跳过 name === "capsule"）。 */
|
||||
function collectWorldSpaceGeometries(sources: THREE.Object3D[]): THREE.BufferGeometry[] {
|
||||
const collected: THREE.BufferGeometry[] = [];
|
||||
const collectMesh = (mesh: THREE.Mesh | THREE.LineSegments) => {
|
||||
try {
|
||||
let geom = mesh.geometry.clone();
|
||||
geom.applyMatrix4(mesh.matrixWorld);
|
||||
if (geom.index) geom = geom.toNonIndexed();
|
||||
const safe = ensureAttributesMinimal(geom);
|
||||
if (safe) collected.push(safe);
|
||||
} catch (e) {
|
||||
console.warn("处理网格时出错：", mesh, e);
|
||||
}
|
||||
};
|
||||
for (const obj of sources) {
|
||||
obj.updateMatrixWorld(true);
|
||||
obj.traverse(c => {
|
||||
const a = c as any;
|
||||
if (
|
||||
(a.isMesh || a.isLineSegments)
|
||||
&& a.geometry
|
||||
&& c.name !== "capsule"
|
||||
&& !isExcludedFromCollider(c)
|
||||
) collectMesh(a);
|
||||
});
|
||||
}
|
||||
return collected;
|
||||
}
|
||||
|
||||
const _rtcTranslation = new THREE.Matrix4();
|
||||
const _rtcCenter = new THREE.Vector3();
|
||||
|
||||
/**
|
||||
* 把合并后的几何收到包围盒中心附近，避免大坐标留在 Float32 顶点里。
|
||||
* 返回平移到原点的中心，调用方需把它编进 bakeInverse。
|
||||
*/
|
||||
function recenterGeometry(geometry: THREE.BufferGeometry, target: THREE.Vector3): THREE.Vector3 {
|
||||
geometry.computeBoundingBox();
|
||||
const box = geometry.boundingBox;
|
||||
if (!box || box.isEmpty()) {
|
||||
target.set(0, 0, 0);
|
||||
return target;
|
||||
}
|
||||
box.getCenter(target);
|
||||
if (target.lengthSq() === 0) return target;
|
||||
geometry.translate(-target.x, -target.y, -target.z);
|
||||
geometry.computeBoundingBox();
|
||||
geometry.computeBoundingSphere();
|
||||
return target;
|
||||
}
|
||||
|
||||
/**
|
||||
* 用合并几何创建线框调试 Mesh，并挂上 acceleratedRaycast。
|
||||
* localSpace 时关闭 matrixAutoUpdate，由外部每帧对齐 follow。
|
||||
*/
|
||||
function makeDebugMesh(geometry: THREE.BufferGeometry, localSpace: boolean): THREE.Mesh {
|
||||
const mesh = new THREE.Mesh(
|
||||
geometry,
|
||||
new THREE.MeshBasicMaterial({
|
||||
opacity: 0.5,
|
||||
transparent: true,
|
||||
wireframe: true,
|
||||
depthTest: true,
|
||||
side: THREE.DoubleSide,
|
||||
}),
|
||||
);
|
||||
mesh.raycast = acceleratedRaycast;
|
||||
if (localSpace) {
|
||||
mesh.matrixAutoUpdate = false;
|
||||
} else {
|
||||
mesh.layers.enable(1);
|
||||
}
|
||||
return mesh;
|
||||
}
|
||||
|
||||
/** build*MergedMesh 的返回值：调试用 mesh + 待挂 BVH 的 geometry。 */
|
||||
export type BuiltMeshCollider = {
|
||||
mesh: THREE.Mesh;
|
||||
geometry: THREE.BufferGeometry;
|
||||
/** 运动学烘焙：inv(source.matrixWorld) * T(rtcCenter)。*/
|
||||
bakeInverse?: THREE.Matrix4;
|
||||
};
|
||||
|
||||
/**
|
||||
* 合并 source 为世界空间静态碰撞网格。
|
||||
* 仅合并几何，不挂 BVH；调用方需再走 attachBoundsTree，否则不能参与碰撞查询。
|
||||
*/
|
||||
export function buildStaticMergedMesh(sources: THREE.Object3D | THREE.Object3D[]): BuiltMeshCollider | null {
|
||||
const list = Array.isArray(sources) ? sources : [sources];
|
||||
let collected = collectWorldSpaceGeometries(list);
|
||||
if (!collected.length) return null;
|
||||
collected = unifiedAttribute(collected);
|
||||
const merged = BufferGeometryUtils.mergeGeometries(collected, false);
|
||||
if (!merged) {
|
||||
console.error("合并几何失败");
|
||||
return null;
|
||||
}
|
||||
return { mesh: makeDebugMesh(merged, false), geometry: merged };
|
||||
}
|
||||
|
||||
/**
|
||||
* 合并 source 为运动学碰撞网格。
|
||||
* 先乘 matrixWorld 落到当前世界 / 局部 ENU，再绕包围盒中心收成小坐标；
|
||||
* 调用方需再走 attachBoundsTree，并每帧用
|
||||
* source.matrixWorld * bakeInverse 跟随。
|
||||
*/
|
||||
export function buildKinematicMergedMesh(source: THREE.Object3D): BuiltMeshCollider | null {
|
||||
source.updateMatrixWorld(true);
|
||||
let collected = collectWorldSpaceGeometries([source]);
|
||||
if (!collected.length) return null;
|
||||
collected = unifiedAttribute(collected);
|
||||
const merged = BufferGeometryUtils.mergeGeometries(collected, false);
|
||||
if (!merged) {
|
||||
console.error("合并运动学几何失败");
|
||||
return null;
|
||||
}
|
||||
const rtcCenter = recenterGeometry(merged, _rtcCenter);
|
||||
const bakeInverse = new THREE.Matrix4().copy(source.matrixWorld).invert();
|
||||
bakeInverse.multiply(_rtcTranslation.makeTranslation(rtcCenter.x, rtcCenter.y, rtcCenter.z));
|
||||
const mesh = makeDebugMesh(merged, true);
|
||||
mesh.matrix.copy(source.matrixWorld).multiply(bakeInverse);
|
||||
mesh.updateMatrixWorld(true);
|
||||
return { mesh, geometry: merged, bakeInverse };
|
||||
}
|
||||
|
||||
/**
|
||||
* 为几何挂上 MeshBVH（写入 geometry.boundsTree）。
|
||||
* useWorker + workerPool 时异步构建，失败回退主线程；onStale 为真则丢弃过期结果。
|
||||
*/
|
||||
export function attachBoundsTree(
|
||||
geometry: THREE.BufferGeometry,
|
||||
options: {
|
||||
useWorker?: boolean;
|
||||
workerPool?: BvhWorkerPool;
|
||||
maxDepth?: number;
|
||||
onReady: (bvh: MeshBVH) => void;
|
||||
onStale?: () => boolean;
|
||||
},
|
||||
): void {
|
||||
const maxDepth = options.maxDepth;
|
||||
const sync = () => {
|
||||
const bvh = maxDepth != null
|
||||
? new MeshBVH(geometry, { maxDepth })
|
||||
: new MeshBVH(geometry);
|
||||
(geometry as any).boundsTree = bvh;
|
||||
options.onReady(bvh);
|
||||
};
|
||||
|
||||
if (!options.useWorker || !options.workerPool) {
|
||||
sync();
|
||||
return;
|
||||
}
|
||||
|
||||
void options.workerPool.generate(geometry).then((bvh) => {
|
||||
if (options.onStale?.()) return;
|
||||
(geometry as any).boundsTree = bvh;
|
||||
options.onReady(bvh);
|
||||
}).catch((error) => {
|
||||
if (options.onStale?.()) return;
|
||||
console.warn("异步构建 BVH 失败，回退到主线程：", error);
|
||||
sync();
|
||||
});
|
||||
}
|
||||
@@ -0,0 +1,111 @@
|
||||
import type * as THREE from "three";
|
||||
import type { MotionType } from "./CollisionWorld";
|
||||
|
||||
/** 从场景节点采集并合并的三角网格。 */
|
||||
export type MeshSourceShape = {
|
||||
kind: "mesh";
|
||||
/** 采集几何的来源；必传。 */
|
||||
source: THREE.Object3D | THREE.Object3D[];
|
||||
mesh?: never;
|
||||
};
|
||||
|
||||
/** 已有（可含 BVH）的碰撞网格。 */
|
||||
export type MeshReadyShape = {
|
||||
kind: "mesh";
|
||||
mesh: THREE.Mesh;
|
||||
source?: never;
|
||||
};
|
||||
|
||||
export type MeshShape = MeshSourceShape | MeshReadyShape;
|
||||
|
||||
export type BoxShape = {
|
||||
kind: "box";
|
||||
halfExtents: THREE.Vector3;
|
||||
/** 世界位置，或相对 follow 的局部偏移。 */
|
||||
position?: THREE.Vector3;
|
||||
quaternion?: THREE.Quaternion;
|
||||
};
|
||||
|
||||
export type SphereShape = {
|
||||
kind: "sphere";
|
||||
radius: number;
|
||||
/** 世界位置，或相对 follow 的局部偏移。 */
|
||||
position?: THREE.Vector3;
|
||||
};
|
||||
|
||||
export type ColliderShape = MeshShape | BoxShape | SphereShape;
|
||||
|
||||
type ColliderDescBase = {
|
||||
groups?: number;
|
||||
mask?: number;
|
||||
useWorker?: boolean;
|
||||
userData?: unknown;
|
||||
};
|
||||
|
||||
export type StaticColliderDesc = ColliderDescBase & {
|
||||
motion: "static";
|
||||
shape: ColliderShape;
|
||||
follow?: never;
|
||||
};
|
||||
|
||||
export type KinematicColliderDesc = ColliderDescBase & {
|
||||
motion: "kinematic";
|
||||
shape: ColliderShape;
|
||||
/** 每帧碰撞体对齐该节点的 matrixWorld；mesh+source 时默认跟 source。 */
|
||||
follow?: THREE.Object3D;
|
||||
};
|
||||
|
||||
type DynamicColliderMaterialFields = {
|
||||
/** 密度，质量 = 体积 × 密度；默认见 DYNAMIC_BODY_DEFAULTS。 */
|
||||
density?: number;
|
||||
/** 弹性，0 不反弹，1 完全弹；默认见 DYNAMIC_BODY_DEFAULTS。 */
|
||||
restitution?: number;
|
||||
/** 摩擦；默认见 DYNAMIC_BODY_DEFAULTS。 */
|
||||
friction?: number;
|
||||
/** 线阻尼，越大滑行越快停；默认见 DYNAMIC_BODY_DEFAULTS。 */
|
||||
linearDamping?: number;
|
||||
/** 角阻尼，越大旋转越快停；默认见 DYNAMIC_BODY_DEFAULTS。 */
|
||||
angularDamping?: number;
|
||||
gravity?: number;
|
||||
velocity?: THREE.Vector3;
|
||||
angularVelocity?: THREE.Vector3;
|
||||
};
|
||||
|
||||
/** 动态碰撞体描述。 */
|
||||
export type DynamicColliderDesc = ColliderDescBase & DynamicColliderMaterialFields & {
|
||||
motion: "dynamic";
|
||||
shape: ColliderShape;
|
||||
follow?: never;
|
||||
/** 可选视觉网格。 */
|
||||
mesh?: THREE.Mesh;
|
||||
/**
|
||||
* 是否由 DynamicBodySystem 创建并推进刚体。
|
||||
* - 默认 true（省略时）：球 / 可推箱子等会创建 DynamicBody
|
||||
* - false：只登记 CollisionWorld，不创建 DynamicBody（车辆底盘由 VehicleRigidBody 推进）
|
||||
*/
|
||||
simulate?: boolean;
|
||||
};
|
||||
|
||||
/** 动态刚体材质默认值。 */
|
||||
export const DYNAMIC_BODY_DEFAULTS = {
|
||||
density: 1,
|
||||
restitution: 0.2,
|
||||
friction: 0.6,
|
||||
linearDamping: 0.4,
|
||||
angularDamping: 0.6,
|
||||
};
|
||||
|
||||
/** 碰撞体创建描述。 */
|
||||
export type ColliderDesc = StaticColliderDesc | KinematicColliderDesc | DynamicColliderDesc;
|
||||
|
||||
/** addCollider 返回的句柄。 */
|
||||
export type ColliderHandle = {
|
||||
id: number;
|
||||
motion: MotionType;
|
||||
/** 运动学跟随目标（若有）。 */
|
||||
follow: THREE.Object3D | null;
|
||||
/** mesh 采集源（若有）。 */
|
||||
source: THREE.Object3D | null;
|
||||
/** 用于 BVH 查询的网格（static/kinematic mesh）。 */
|
||||
collisionMesh: THREE.Mesh | null;
|
||||
};
|
||||
@@ -0,0 +1,85 @@
|
||||
import * as THREE from "three";
|
||||
import { ContactManifold } from "./ContactManifold";
|
||||
import { ContactPoint } from "./ContactPoint";
|
||||
import { normalClass } from "./contactReducer";
|
||||
|
||||
const MATCH_NORMAL = 0.95; // 法线点积低于此值视为不同流形
|
||||
const MATCH_DISTANCE = 0.08; // 局部接触点匹配距离上限
|
||||
|
||||
/** findMatch 的命中结果。 */
|
||||
type MatchResult = {
|
||||
point: ContactPoint; // 上一帧匹配到的接触点
|
||||
normal: THREE.Vector3; // 上一帧流形法线
|
||||
};
|
||||
|
||||
/**
|
||||
* 帧间接触点匹配，用于冲量热启动（warm start）。
|
||||
* 求解前 match 继承冲量；求解后 save 供下一帧使用。
|
||||
*/
|
||||
export class ContactCache {
|
||||
private prev: ContactManifold[] = []; // 上一帧流形快照
|
||||
|
||||
/**
|
||||
* 将上一帧冲量拷到本帧对应接触点。
|
||||
* 无匹配、或支撑面 ↔ 墙面切换时清零，避免错误热启动。
|
||||
*/
|
||||
match(manifolds: ContactManifold[]): void {
|
||||
for (const manifold of manifolds) {
|
||||
for (const point of manifold.contacts) {
|
||||
const prev = this.findMatch(manifold.normal, point);
|
||||
if (!prev) {
|
||||
point.resetImpulses();
|
||||
continue;
|
||||
}
|
||||
// 法线类别突变（地面 ↔ 墙）时冲量不可复用
|
||||
const prevClass = normalClass(prev.normal);
|
||||
const nextClass = normalClass(manifold.normal);
|
||||
if (
|
||||
(prevClass === "support" && nextClass === "wall") ||
|
||||
(prevClass === "wall" && nextClass === "support")
|
||||
) {
|
||||
point.resetImpulses();
|
||||
continue;
|
||||
}
|
||||
point.normalImpulse = prev.point.normalImpulse;
|
||||
point.tangentImpulse1 = prev.point.tangentImpulse1;
|
||||
point.tangentImpulse2 = prev.point.tangentImpulse2;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/** 深拷贝本帧流形与接触冲量，供下一帧 match。 */
|
||||
save(manifolds: ContactManifold[]): void {
|
||||
this.prev = manifolds.map((src) => {
|
||||
const copy = new ContactManifold();
|
||||
copy.normal.copy(src.normal);
|
||||
copy.normalSum.copy(src.normalSum);
|
||||
copy.contacts = src.contacts.map((p) => {
|
||||
const c = new ContactPoint();
|
||||
c.copyFrom(p);
|
||||
return c;
|
||||
});
|
||||
return copy;
|
||||
});
|
||||
}
|
||||
|
||||
/**
|
||||
* 在上一帧中找法线接近且局部点最近的接触。
|
||||
* 用刚体局部坐标匹配，避免物体移动后面世界点对不上。
|
||||
*/
|
||||
private findMatch(normal: THREE.Vector3, point: ContactPoint): MatchResult | null {
|
||||
let best: MatchResult | null = null;
|
||||
let bestDist = MATCH_DISTANCE * MATCH_DISTANCE;
|
||||
for (const manifold of this.prev) {
|
||||
if (manifold.normal.dot(normal) < MATCH_NORMAL) continue;
|
||||
for (const prev of manifold.contacts) {
|
||||
const d = prev.localPoint.distanceToSquared(point.localPoint);
|
||||
if (d < bestDist) {
|
||||
bestDist = d;
|
||||
best = { point: prev, normal: manifold.normal };
|
||||
}
|
||||
}
|
||||
}
|
||||
return best;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,15 @@
|
||||
import * as THREE from "three";
|
||||
import { ContactPoint } from "./ContactPoint";
|
||||
|
||||
/** 同一法线簇下的接触点集合。 */
|
||||
export class ContactManifold {
|
||||
normal = new THREE.Vector3();
|
||||
normalSum = new THREE.Vector3();
|
||||
contacts: ContactPoint[] = [];
|
||||
|
||||
clear(): void {
|
||||
this.contacts.length = 0;
|
||||
this.normal.set(0, 1, 0);
|
||||
this.normalSum.set(0, 0, 0);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,48 @@
|
||||
import * as THREE from "three";
|
||||
|
||||
/** 标定用特征半尺寸；skin/slop 按 extent 相对此值缩放。 */
|
||||
export const CONTACT_REF_EXTENT = 0.2;
|
||||
|
||||
export const CONTACT_SKIN = 0.002;
|
||||
|
||||
/** 按刚体特征尺寸缩放接触皮肤，避免小物体相对厚度过大。 */
|
||||
export function contactSkinForExtent(extent: number): number {
|
||||
const s = Math.max(1e-4, extent) / CONTACT_REF_EXTENT;
|
||||
return CONTACT_SKIN * s;
|
||||
}
|
||||
|
||||
export type RawContact = {
|
||||
point: THREE.Vector3;
|
||||
normal: THREE.Vector3;
|
||||
penetration: number;
|
||||
};
|
||||
|
||||
/** 约束求解用的单个接触点。 */
|
||||
export class ContactPoint {
|
||||
worldPoint = new THREE.Vector3();
|
||||
localPoint = new THREE.Vector3();
|
||||
penetration = 0;
|
||||
biasVelocity = 0;
|
||||
normalImpulse = 0;
|
||||
tangentImpulse1 = 0;
|
||||
tangentImpulse2 = 0;
|
||||
tangent1 = new THREE.Vector3();
|
||||
tangent2 = new THREE.Vector3();
|
||||
|
||||
copyFrom(other: ContactPoint): void {
|
||||
this.worldPoint.copy(other.worldPoint);
|
||||
this.localPoint.copy(other.localPoint);
|
||||
this.penetration = other.penetration;
|
||||
this.normalImpulse = other.normalImpulse;
|
||||
this.tangentImpulse1 = other.tangentImpulse1;
|
||||
this.tangentImpulse2 = other.tangentImpulse2;
|
||||
this.tangent1.copy(other.tangent1);
|
||||
this.tangent2.copy(other.tangent2);
|
||||
}
|
||||
|
||||
resetImpulses(): void {
|
||||
this.normalImpulse = 0;
|
||||
this.tangentImpulse1 = 0;
|
||||
this.tangentImpulse2 = 0;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,201 @@
|
||||
import * as THREE from "three";
|
||||
import type { ImpulseBody } from "../solver/ImpulseBody";
|
||||
import { ContactManifold } from "./ContactManifold";
|
||||
import { ContactPoint, CONTACT_REF_EXTENT, contactSkinForExtent, type RawContact } from "./ContactPoint";
|
||||
|
||||
const NORMAL_MERGE = 0.98; // 法线点积高于此值并入同一流形
|
||||
const MAX_POINTS = 4; // 单流形最多保留的接触点
|
||||
const SUPPORT_Y = 0.7; // ny 以上视为支撑面（地面）
|
||||
const WALL_Y = 0.35; // ny 以下视为墙面
|
||||
|
||||
const _invQuat = new THREE.Quaternion();
|
||||
const _ab = new THREE.Vector3();
|
||||
const _ac = new THREE.Vector3();
|
||||
const _ap = new THREE.Vector3();
|
||||
const _n = new THREE.Vector3();
|
||||
|
||||
/**
|
||||
* 将原始接触按法线聚类，并裁成至多 4 个支撑点。
|
||||
* 返回写入后的 out（长度收束为有效流形数）。
|
||||
*/
|
||||
export function reduceContacts(
|
||||
raw: RawContact[],
|
||||
count: number,
|
||||
body: ImpulseBody,
|
||||
out: ContactManifold[],
|
||||
): ContactManifold[] {
|
||||
for (const m of out) m.clear();
|
||||
let manifoldCount = 0;
|
||||
|
||||
// 按法线合并进流形，并累计 normalSum
|
||||
const skin = contactSkinForExtent(body.characteristicExtent?.() ?? CONTACT_REF_EXTENT);
|
||||
for (let r = 0; r < count; r++) {
|
||||
const contact = raw[r];
|
||||
if (contact.penetration < -skin || contact.normal.lengthSq() < 1e-8) continue;
|
||||
_n.copy(contact.normal).normalize();
|
||||
let cluster = -1;
|
||||
for (let i = 0; i < manifoldCount; i++) {
|
||||
if (out[i].normal.dot(_n) > NORMAL_MERGE) {
|
||||
cluster = i;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (cluster < 0) {
|
||||
if (manifoldCount === out.length) {
|
||||
out.push(new ContactManifold());
|
||||
}
|
||||
cluster = manifoldCount;
|
||||
manifoldCount += 1;
|
||||
out[cluster].normalSum.copy(_n);
|
||||
out[cluster].normal.copy(_n);
|
||||
} else {
|
||||
out[cluster].normalSum.add(_n);
|
||||
out[cluster].normal.copy(out[cluster].normalSum).normalize();
|
||||
}
|
||||
out[cluster].contacts.push(makePoint(contact, body));
|
||||
}
|
||||
|
||||
// 定稿法线、裁点、建切向基
|
||||
for (let i = 0; i < manifoldCount; i++) {
|
||||
const manifold = out[i];
|
||||
if (manifold.normalSum.lengthSq() < 1e-8) manifold.normal.set(0, 1, 0);
|
||||
else manifold.normal.copy(manifold.normalSum).normalize();
|
||||
if (manifold.contacts.length > MAX_POINTS) {
|
||||
manifold.contacts = pickSupportPoints(manifold.contacts);
|
||||
}
|
||||
for (const point of manifold.contacts) {
|
||||
buildTangents(manifold.normal, point);
|
||||
}
|
||||
}
|
||||
|
||||
out.length = manifoldCount;
|
||||
stabilizeEdgeManifolds(out);
|
||||
return out;
|
||||
}
|
||||
|
||||
/** 按法线竖直分量分类：支撑面 / 墙 / 斜面。 */
|
||||
export function normalClass(normal: THREE.Vector3): "support" | "wall" | "slant" {
|
||||
if (normal.y >= SUPPORT_Y) return "support";
|
||||
if (normal.y <= WALL_Y) return "wall";
|
||||
return "slant";
|
||||
}
|
||||
|
||||
/**
|
||||
* 边角同时碰到地面与墙时丢掉斜面流形，避免错误推力。
|
||||
* 若没有支撑面，则把斜面法线抬到至少 SUPPORT_Y，减轻卡边抖动。
|
||||
*/
|
||||
function stabilizeEdgeManifolds(manifolds: ContactManifold[]): void {
|
||||
let hasSupport = false;
|
||||
let hasWall = false;
|
||||
for (const m of manifolds) {
|
||||
const cls = normalClass(m.normal);
|
||||
if (cls === "support") hasSupport = true;
|
||||
else if (cls === "wall") hasWall = true;
|
||||
}
|
||||
|
||||
if (hasSupport && hasWall) {
|
||||
let write = 0;
|
||||
for (let i = 0; i < manifolds.length; i++) {
|
||||
if (normalClass(manifolds[i].normal) === "slant") continue;
|
||||
if (write !== i) manifolds[write] = manifolds[i];
|
||||
write += 1;
|
||||
}
|
||||
manifolds.length = write;
|
||||
return;
|
||||
}
|
||||
|
||||
if (!hasSupport) {
|
||||
for (const m of manifolds) {
|
||||
if (normalClass(m.normal) !== "slant") continue;
|
||||
m.normal.y = Math.max(m.normal.y, SUPPORT_Y);
|
||||
m.normal.normalize();
|
||||
m.normalSum.copy(m.normal);
|
||||
for (const point of m.contacts) {
|
||||
buildTangents(m.normal, point);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/** 由原始接触生成 ContactPoint，并写入刚体局部坐标（供帧间匹配）。 */
|
||||
function makePoint(raw: RawContact, body: ImpulseBody): ContactPoint {
|
||||
const point = new ContactPoint();
|
||||
point.worldPoint.copy(raw.point);
|
||||
point.penetration = raw.penetration;
|
||||
_invQuat.copy(body.quaternion).invert();
|
||||
point.localPoint.subVectors(raw.point, body.position).applyQuaternion(_invQuat);
|
||||
return point;
|
||||
}
|
||||
|
||||
/**
|
||||
* 从过多接触点中挑最多 4 个：最深穿透 → 最远点 → 最大面积第三点 → 离平面最远第四点。
|
||||
* 用于稳定盒/车身 resting 接触。
|
||||
*/
|
||||
function pickSupportPoints(points: ContactPoint[]): ContactPoint[] {
|
||||
if (points.length <= MAX_POINTS) return points;
|
||||
|
||||
// p0：穿透最深
|
||||
let i0 = 0;
|
||||
for (let i = 1; i < points.length; i++) {
|
||||
if (points[i].penetration > points[i0].penetration) i0 = i;
|
||||
}
|
||||
const p0 = points[i0];
|
||||
|
||||
// p1：离 p0 最远
|
||||
let i1 = i0;
|
||||
let best = -1;
|
||||
for (let i = 0; i < points.length; i++) {
|
||||
const d = points[i].worldPoint.distanceToSquared(p0.worldPoint);
|
||||
if (d > best) {
|
||||
best = d;
|
||||
i1 = i;
|
||||
}
|
||||
}
|
||||
const p1 = points[i1];
|
||||
|
||||
// p2：与 p0p1 构成面积最大
|
||||
let i2 = i0;
|
||||
best = -1;
|
||||
_ab.subVectors(p1.worldPoint, p0.worldPoint);
|
||||
for (let i = 0; i < points.length; i++) {
|
||||
_ap.subVectors(points[i].worldPoint, p0.worldPoint);
|
||||
const area = _ap.cross(_ab).lengthSq();
|
||||
if (area > best) {
|
||||
best = area;
|
||||
i2 = i;
|
||||
}
|
||||
}
|
||||
const p2 = points[i2];
|
||||
|
||||
// p3：离 p0p1p2 平面最远（略加权穿透）
|
||||
let i3 = i0;
|
||||
best = -1;
|
||||
_ab.subVectors(p1.worldPoint, p0.worldPoint);
|
||||
_ac.subVectors(p2.worldPoint, p0.worldPoint);
|
||||
_n.copy(_ab).cross(_ac);
|
||||
for (let i = 0; i < points.length; i++) {
|
||||
if (i === i0 || i === i1 || i === i2) continue;
|
||||
_ap.subVectors(points[i].worldPoint, p0.worldPoint);
|
||||
const d = Math.abs(_ap.dot(_n));
|
||||
const score = d + points[i].penetration * 0.01;
|
||||
if (score > best) {
|
||||
best = score;
|
||||
i3 = i;
|
||||
}
|
||||
}
|
||||
|
||||
const picked = [p0];
|
||||
if (i1 !== i0) picked.push(p1);
|
||||
if (i2 !== i0 && i2 !== i1) picked.push(p2);
|
||||
if (i3 !== i0 && i3 !== i1 && i3 !== i2 && picked.length < MAX_POINTS) picked.push(points[i3]);
|
||||
return picked;
|
||||
}
|
||||
|
||||
/** 由流形法线构造正交切向基（摩擦两轴）。 */
|
||||
function buildTangents(normal: THREE.Vector3, point: ContactPoint): void {
|
||||
if (Math.abs(normal.y) < 0.99) point.tangent1.set(0, 1, 0).cross(normal);
|
||||
else point.tangent1.set(1, 0, 0).cross(normal); // 法线近竖直时换参考轴
|
||||
if (point.tangent1.lengthSq() < 1e-8) point.tangent1.set(0, 0, 1).cross(normal);
|
||||
point.tangent1.normalize();
|
||||
point.tangent2.copy(normal).cross(point.tangent1).normalize();
|
||||
}
|
||||
@@ -0,0 +1,20 @@
|
||||
/** 碰撞业务分组 bitmask（与 MotionType 正交）。 */
|
||||
export const CollisionGroup = {
|
||||
ALL: 0xffff,
|
||||
/** 场景静态 / 运动学网格等默认层。 */
|
||||
DEFAULT: 1 << 0,
|
||||
/** 人物相关（预留）。 */
|
||||
CHARACTER: 1 << 1,
|
||||
/** 车辆底盘盒。 */
|
||||
VEHICLE: 1 << 2,
|
||||
/** 动态碎片（球等）。 */
|
||||
DEBRIS: 1 << 3,
|
||||
} as const;
|
||||
|
||||
/** 人物胶囊 / 相机射线的单向查询目标。 */
|
||||
export const CHARACTER_QUERY_MASK = CollisionGroup.DEFAULT;
|
||||
|
||||
/** 全部分组掩码。 */
|
||||
export const ALL_COLLISION_MASK = CollisionGroup.ALL;
|
||||
|
||||
export type CollisionGroupBits = (typeof CollisionGroup)[keyof typeof CollisionGroup];
|
||||
@@ -0,0 +1,31 @@
|
||||
export type { MotionType, CollisionCollider, CollisionShape, AddColliderOptions, MeshQueryFilter, MeshQueryOptions } from "./CollisionWorld";
|
||||
export { CollisionWorld } from "./CollisionWorld";
|
||||
export {
|
||||
CollisionGroup,
|
||||
CHARACTER_QUERY_MASK,
|
||||
ALL_COLLISION_MASK,
|
||||
type CollisionGroupBits,
|
||||
} from "./groups";
|
||||
export type {
|
||||
ColliderDesc,
|
||||
ColliderHandle,
|
||||
ColliderShape,
|
||||
MeshShape,
|
||||
BoxShape,
|
||||
SphereShape,
|
||||
StaticColliderDesc,
|
||||
KinematicColliderDesc,
|
||||
DynamicColliderDesc,
|
||||
} from "./colliderDesc";
|
||||
export { DYNAMIC_BODY_DEFAULTS } from "./colliderDesc";
|
||||
export { DynamicBody } from "./DynamicBody";
|
||||
export type { DynamicBodyKind, DynamicBodyMaterialOptions } from "./DynamicBody";
|
||||
export { DynamicSphereBody, isSphereBody } from "./DynamicSphere";
|
||||
export { DynamicBoxBody, isBoxBody } from "./DynamicBox";
|
||||
export {
|
||||
buildStaticMergedMesh,
|
||||
buildKinematicMergedMesh,
|
||||
attachBoundsTree,
|
||||
ensureAttributesMinimal,
|
||||
unifiedAttribute,
|
||||
} from "./colliderBuild";
|
||||
@@ -0,0 +1,223 @@
|
||||
import * as THREE from "three";
|
||||
import { impulseDenominator } from "../utils/vehiclePhysics/vehicleMath";
|
||||
import type { ImpulseBody } from "./solver/ImpulseBody";
|
||||
|
||||
const EPS = 1e-8; // 退化轴 / 零长度下限
|
||||
const AXIS_EPS = 1e-10; // 平行边叉积视为退化
|
||||
const BAUMGARTE = 0.8; // 每帧推出重叠的比例
|
||||
const RESTITUTION = 0.2; // 车车法向弹性
|
||||
const FRICTION = 0.4; // 切向摩擦相对法向冲量的上限
|
||||
|
||||
/** OBB–OBB 解算所需的刚体字段（扩展共享 ImpulseBody）。 */
|
||||
export type ObbRigidBody = ImpulseBody & {
|
||||
halfExtents: THREE.Vector3; // 碰撞盒半边长
|
||||
};
|
||||
|
||||
/** OBB SAT / 接触冲量的预分配临时量。 */
|
||||
export type ObbObbTemps = {
|
||||
ax: THREE.Vector3; // A 盒世界轴 X
|
||||
ay: THREE.Vector3;
|
||||
az: THREE.Vector3;
|
||||
bx: THREE.Vector3; // B 盒世界轴 X
|
||||
by: THREE.Vector3;
|
||||
bz: THREE.Vector3;
|
||||
axis: THREE.Vector3; // 当前测试分离轴
|
||||
bestN: THREE.Vector3; // 最小平移法线（A → B）
|
||||
tangent: THREE.Vector3; // 摩擦切向
|
||||
invQuatA: THREE.Quaternion;
|
||||
invQuatB: THREE.Quaternion;
|
||||
local: THREE.Vector3; // 支撑点局部缓冲
|
||||
supportA: THREE.Vector3; // A 沿法线的支撑点
|
||||
supportB: THREE.Vector3; // B 沿反法线的支撑点
|
||||
contact: THREE.Vector3; // 世界接触点（两支撑中点）
|
||||
velA: THREE.Vector3; // 接触点处 A 速度
|
||||
velB: THREE.Vector3;
|
||||
rel: THREE.Vector3; // 质心差 / 相对速度缓冲
|
||||
impulse: THREE.Vector3;
|
||||
r: THREE.Vector3; // impulseDenominator 用
|
||||
gcross: THREE.Vector3;
|
||||
localI: THREE.Vector3;
|
||||
worldI: THREE.Vector3;
|
||||
invQuat: THREE.Quaternion;
|
||||
};
|
||||
|
||||
/** 创建 OBB 碰撞临时量。 */
|
||||
export function createObbObbTemps(): ObbObbTemps {
|
||||
return {
|
||||
ax: new THREE.Vector3(),
|
||||
ay: new THREE.Vector3(),
|
||||
az: new THREE.Vector3(),
|
||||
bx: new THREE.Vector3(),
|
||||
by: new THREE.Vector3(),
|
||||
bz: new THREE.Vector3(),
|
||||
axis: new THREE.Vector3(),
|
||||
bestN: new THREE.Vector3(),
|
||||
tangent: new THREE.Vector3(),
|
||||
invQuatA: new THREE.Quaternion(),
|
||||
invQuatB: new THREE.Quaternion(),
|
||||
local: new THREE.Vector3(),
|
||||
supportA: new THREE.Vector3(),
|
||||
supportB: new THREE.Vector3(),
|
||||
contact: new THREE.Vector3(),
|
||||
velA: new THREE.Vector3(),
|
||||
velB: new THREE.Vector3(),
|
||||
rel: new THREE.Vector3(),
|
||||
impulse: new THREE.Vector3(),
|
||||
r: new THREE.Vector3(),
|
||||
gcross: new THREE.Vector3(),
|
||||
localI: new THREE.Vector3(),
|
||||
worldI: new THREE.Vector3(),
|
||||
invQuat: new THREE.Quaternion(),
|
||||
};
|
||||
}
|
||||
|
||||
/** 盒在轴 n 上的投影半径（半宽之和）。 */
|
||||
function radiusOnAxis(ax: THREE.Vector3, ay: THREE.Vector3, az: THREE.Vector3, half: THREE.Vector3, n: THREE.Vector3): number {
|
||||
return half.x * Math.abs(n.dot(ax)) + half.y * Math.abs(n.dot(ay)) + half.z * Math.abs(n.dot(az));
|
||||
}
|
||||
|
||||
/**
|
||||
* 盒在 dir 方向上的支撑点（取各轴符号 * 半边，面法向时落在对应面心附近）。
|
||||
*/
|
||||
function supportPoint(
|
||||
pos: THREE.Vector3,
|
||||
quat: THREE.Quaternion,
|
||||
invQuat: THREE.Quaternion,
|
||||
half: THREE.Vector3,
|
||||
dir: THREE.Vector3,
|
||||
local: THREE.Vector3,
|
||||
out: THREE.Vector3,
|
||||
): THREE.Vector3 {
|
||||
local.copy(dir).applyQuaternion(invQuat);
|
||||
out.set(
|
||||
Math.sign(local.x) * half.x,
|
||||
Math.sign(local.y) * half.y,
|
||||
Math.sign(local.z) * half.z,
|
||||
);
|
||||
return out.applyQuaternion(quat).add(pos);
|
||||
}
|
||||
|
||||
/**
|
||||
* 两车底盘 OBB：SAT 最小平移推出，再按质量在接触点施法向 / 摩擦冲量。
|
||||
* `temps.bestN` 指向从 A 到 B；有重叠并处理则返回 true。
|
||||
*/
|
||||
export function resolveObbObb(a: ObbRigidBody, b: ObbRigidBody, temps: ObbObbTemps): boolean {
|
||||
// 两盒局部轴变到世界
|
||||
temps.ax.set(1, 0, 0).applyQuaternion(a.quaternion);
|
||||
temps.ay.set(0, 1, 0).applyQuaternion(a.quaternion);
|
||||
temps.az.set(0, 0, 1).applyQuaternion(a.quaternion);
|
||||
temps.bx.set(1, 0, 0).applyQuaternion(b.quaternion);
|
||||
temps.by.set(0, 1, 0).applyQuaternion(b.quaternion);
|
||||
temps.bz.set(0, 0, 1).applyQuaternion(b.quaternion);
|
||||
|
||||
let minOverlap = Infinity;
|
||||
/** 测试一根分离轴：无重叠返回 false；否则更新最小平移轴。 */
|
||||
const tryAxis = (x: number, y: number, z: number): boolean => {
|
||||
temps.axis.set(x, y, z);
|
||||
const lenSq = temps.axis.lengthSq();
|
||||
if (lenSq < AXIS_EPS) return true; // 平行边叉积退化，跳过
|
||||
temps.axis.multiplyScalar(1 / Math.sqrt(lenSq));
|
||||
// 重叠深度 = 两投影半径之和 - 质心差在轴上的投影
|
||||
const overlap =
|
||||
radiusOnAxis(temps.ax, temps.ay, temps.az, a.halfExtents, temps.axis)
|
||||
+ radiusOnAxis(temps.bx, temps.by, temps.bz, b.halfExtents, temps.axis)
|
||||
- Math.abs(temps.axis.dot(temps.rel.subVectors(b.position, a.position)));
|
||||
if (overlap <= EPS) return false; // 分离
|
||||
if (overlap < minOverlap) {
|
||||
minOverlap = overlap;
|
||||
temps.bestN.copy(temps.axis);
|
||||
// 法线统一为 A → B
|
||||
if (temps.bestN.dot(temps.rel) < 0) temps.bestN.negate();
|
||||
}
|
||||
return true;
|
||||
};
|
||||
|
||||
// 面法线：A、B 各 3 轴
|
||||
if (!tryAxis(temps.ax.x, temps.ax.y, temps.ax.z)) return false;
|
||||
if (!tryAxis(temps.ay.x, temps.ay.y, temps.ay.z)) return false;
|
||||
if (!tryAxis(temps.az.x, temps.az.y, temps.az.z)) return false;
|
||||
if (!tryAxis(temps.bx.x, temps.bx.y, temps.bx.z)) return false;
|
||||
if (!tryAxis(temps.by.x, temps.by.y, temps.by.z)) return false;
|
||||
if (!tryAxis(temps.bz.x, temps.bz.y, temps.bz.z)) return false;
|
||||
|
||||
// 边叉积：A 的 3 边 × B 的 3 边
|
||||
temps.axis.copy(temps.ax).cross(temps.bx);
|
||||
if (!tryAxis(temps.axis.x, temps.axis.y, temps.axis.z)) return false;
|
||||
temps.axis.copy(temps.ax).cross(temps.by);
|
||||
if (!tryAxis(temps.axis.x, temps.axis.y, temps.axis.z)) return false;
|
||||
temps.axis.copy(temps.ax).cross(temps.bz);
|
||||
if (!tryAxis(temps.axis.x, temps.axis.y, temps.axis.z)) return false;
|
||||
temps.axis.copy(temps.ay).cross(temps.bx);
|
||||
if (!tryAxis(temps.axis.x, temps.axis.y, temps.axis.z)) return false;
|
||||
temps.axis.copy(temps.ay).cross(temps.by);
|
||||
if (!tryAxis(temps.axis.x, temps.axis.y, temps.axis.z)) return false;
|
||||
temps.axis.copy(temps.ay).cross(temps.bz);
|
||||
if (!tryAxis(temps.axis.x, temps.axis.y, temps.axis.z)) return false;
|
||||
temps.axis.copy(temps.az).cross(temps.bx);
|
||||
if (!tryAxis(temps.axis.x, temps.axis.y, temps.axis.z)) return false;
|
||||
temps.axis.copy(temps.az).cross(temps.by);
|
||||
if (!tryAxis(temps.axis.x, temps.axis.y, temps.axis.z)) return false;
|
||||
temps.axis.copy(temps.az).cross(temps.bz);
|
||||
if (!tryAxis(temps.axis.x, temps.axis.y, temps.axis.z)) return false;
|
||||
|
||||
if (!Number.isFinite(minOverlap)) return false;
|
||||
|
||||
// 位置修正：深度钳制后按 invMass 分配，乘 Baumgarte
|
||||
const minHalf = Math.min(
|
||||
a.halfExtents.x, a.halfExtents.y, a.halfExtents.z,
|
||||
b.halfExtents.x, b.halfExtents.y, b.halfExtents.z,
|
||||
);
|
||||
const depth = Math.min(minOverlap, minHalf * 2) * BAUMGARTE;
|
||||
const invSum = a.invMass + b.invMass;
|
||||
if (invSum <= EPS || depth <= EPS) return false;
|
||||
|
||||
// 接触点 ≈ 两盒沿法线方向支撑点的中点
|
||||
temps.invQuatA.copy(a.quaternion).invert();
|
||||
temps.invQuatB.copy(b.quaternion).invert();
|
||||
supportPoint(a.position, a.quaternion, temps.invQuatA, a.halfExtents, temps.bestN, temps.local, temps.supportA);
|
||||
supportPoint(b.position, b.quaternion, temps.invQuatB, b.halfExtents, temps.rel.copy(temps.bestN).negate(), temps.local, temps.supportB);
|
||||
temps.contact.copy(temps.supportA).add(temps.supportB).multiplyScalar(0.5);
|
||||
|
||||
a.position.addScaledVector(temps.bestN, -depth * (a.invMass / invSum));
|
||||
b.position.addScaledVector(temps.bestN, depth * (b.invMass / invSum));
|
||||
|
||||
// 法向冲量：仅处理接近中的相对速度
|
||||
a.getVelocityAtPoint(temps.contact, temps.velA);
|
||||
b.getVelocityAtPoint(temps.contact, temps.velB);
|
||||
const relN = temps.velB.dot(temps.bestN) - temps.velA.dot(temps.bestN);
|
||||
if (relN >= 0) return true;
|
||||
|
||||
const denomN =
|
||||
impulseDenominator(a, temps.contact, temps.bestN, temps.r, temps.gcross, temps.localI, temps.worldI, temps.invQuat)
|
||||
+ impulseDenominator(b, temps.contact, temps.bestN, temps.r, temps.gcross, temps.localI, temps.worldI, temps.invQuat);
|
||||
if (denomN <= EPS) return true;
|
||||
|
||||
const jn = -(1 + RESTITUTION) * relN / denomN;
|
||||
temps.impulse.copy(temps.bestN).multiplyScalar(-jn);
|
||||
a.applyImpulseAtPoint(temps.impulse, temps.contact);
|
||||
temps.impulse.copy(temps.bestN).multiplyScalar(jn);
|
||||
b.applyImpulseAtPoint(temps.impulse, temps.contact);
|
||||
|
||||
// 库仑摩擦：切向相对速度，冲量钳在 μ|jn|
|
||||
a.getVelocityAtPoint(temps.contact, temps.velA);
|
||||
b.getVelocityAtPoint(temps.contact, temps.velB);
|
||||
temps.rel.subVectors(temps.velA, temps.velB);
|
||||
temps.rel.addScaledVector(temps.bestN, -temps.rel.dot(temps.bestN)); // 去掉法向分量
|
||||
const tLen = temps.rel.length();
|
||||
if (tLen <= EPS) return true;
|
||||
temps.tangent.copy(temps.rel).multiplyScalar(1 / tLen);
|
||||
|
||||
const denomT =
|
||||
impulseDenominator(a, temps.contact, temps.tangent, temps.r, temps.gcross, temps.localI, temps.worldI, temps.invQuat)
|
||||
+ impulseDenominator(b, temps.contact, temps.tangent, temps.r, temps.gcross, temps.localI, temps.worldI, temps.invQuat);
|
||||
if (denomT <= EPS) return true;
|
||||
|
||||
let jt = -tLen / denomT;
|
||||
const maxF = FRICTION * Math.abs(jn);
|
||||
jt = Math.max(-maxF, Math.min(maxF, jt));
|
||||
temps.impulse.copy(temps.tangent).multiplyScalar(jt);
|
||||
a.applyImpulseAtPoint(temps.impulse, temps.contact);
|
||||
temps.impulse.negate();
|
||||
b.applyImpulseAtPoint(temps.impulse, temps.contact);
|
||||
return true;
|
||||
}
|
||||
@@ -0,0 +1,56 @@
|
||||
import type { ContactManifold } from "../contacts/ContactManifold";
|
||||
import type { ImpulseBody } from "./ImpulseBody";
|
||||
import { solveFrictionConstraint, warmStartFriction } from "./frictionConstraint";
|
||||
import {
|
||||
prepareNormalConstraint,
|
||||
solveNormalConstraint,
|
||||
warmStartNormal,
|
||||
} from "./normalConstraint";
|
||||
|
||||
/**
|
||||
* 共享接触冲量求解器：法向 + 摩擦锥，可选热启动。
|
||||
* 人物胶囊不走此核（位置推出）；轮悬架 / 轮胎仍在车辆模块。
|
||||
*/
|
||||
export class ContactImpulseSolver {
|
||||
velocityIterations = 8; // 速度迭代次数
|
||||
warmStart = true; // 是否复用上一帧冲量做热启动
|
||||
|
||||
/**
|
||||
* 对单个刚体与一组流形做速度级求解。
|
||||
* 顺序：准备 bias → 热启动 → 交替解法向 / 摩擦。
|
||||
*/
|
||||
solveVelocity(
|
||||
body: ImpulseBody,
|
||||
manifolds: ContactManifold[],
|
||||
dt: number,
|
||||
): void {
|
||||
if (!manifolds.length) return;
|
||||
|
||||
// 写入弹性与穿透修正用的 biasVelocity
|
||||
for (const manifold of manifolds) {
|
||||
for (const point of manifold.contacts) {
|
||||
prepareNormalConstraint(body, manifold, point, dt);
|
||||
}
|
||||
}
|
||||
|
||||
// 先施加缓存冲量，减少 resting 抖动
|
||||
if (this.warmStart) {
|
||||
for (const manifold of manifolds) {
|
||||
for (const point of manifold.contacts) {
|
||||
warmStartNormal(body, manifold, point);
|
||||
warmStartFriction(body, point);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// PGS：每轮先法向后摩擦
|
||||
for (let i = 0; i < this.velocityIterations; i++) {
|
||||
for (const manifold of manifolds) {
|
||||
for (const point of manifold.contacts) {
|
||||
solveNormalConstraint(body, manifold, point);
|
||||
solveFrictionConstraint(body, manifold, point);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,28 @@
|
||||
import type * as THREE from "three";
|
||||
|
||||
/**
|
||||
* 接触冲量求解所需的刚体接口。
|
||||
*/
|
||||
export type ImpulseBody = {
|
||||
/** 世界质心位置。 */
|
||||
position: THREE.Vector3;
|
||||
/** 世界朝向。 */
|
||||
quaternion: THREE.Quaternion;
|
||||
/** 质量倒数。 */
|
||||
invMass: number;
|
||||
/** 局部惯量倒数对角元。 */
|
||||
invInertia: THREE.Vector3;
|
||||
/** 摩擦系数 μ；缺省由摩擦约束使用默认值。 */
|
||||
friction?: number;
|
||||
/** 弹性，0 不反弹，1 完全弹；缺省 0。 */
|
||||
restitution?: number;
|
||||
/**
|
||||
* 特征半尺寸（用于缩放接触 skin / 穿透 slop）。
|
||||
* 缺省按 CONTACT_REF_EXTENT，不缩放。
|
||||
*/
|
||||
characteristicExtent?: () => number;
|
||||
/** 读取世界点处速度：v + ω × r。 */
|
||||
getVelocityAtPoint(point: THREE.Vector3, out: THREE.Vector3): THREE.Vector3;
|
||||
/** 在世界点施加冲量（线速度 + 角速度）。 */
|
||||
applyImpulseAtPoint(impulse: THREE.Vector3, point: THREE.Vector3): void;
|
||||
};
|
||||
@@ -0,0 +1,73 @@
|
||||
import * as THREE from "three";
|
||||
import { effectiveMass } from "../../utils/vehiclePhysics/vehicleMath";
|
||||
import type { ContactManifold } from "../contacts/ContactManifold";
|
||||
import type { ContactPoint } from "../contacts/ContactPoint";
|
||||
import type { ImpulseBody } from "./ImpulseBody";
|
||||
|
||||
const DEFAULT_FRICTION = 0.6; // body.friction 未设时的库仑系数 μ
|
||||
|
||||
const _r = new THREE.Vector3();
|
||||
const _gcross = new THREE.Vector3();
|
||||
const _local = new THREE.Vector3();
|
||||
const _world = new THREE.Vector3();
|
||||
const _invQuat = new THREE.Quaternion();
|
||||
const _vel = new THREE.Vector3();
|
||||
const _impulse = new THREE.Vector3();
|
||||
|
||||
/** 热启动：施加上一帧累计的切向冲量。 */
|
||||
export function warmStartFriction(
|
||||
body: ImpulseBody,
|
||||
point: ContactPoint,
|
||||
): void {
|
||||
if (point.tangentImpulse1 === 0 && point.tangentImpulse2 === 0) return;
|
||||
_impulse.copy(point.tangent1).multiplyScalar(point.tangentImpulse1);
|
||||
_impulse.addScaledVector(point.tangent2, point.tangentImpulse2);
|
||||
body.applyImpulseAtPoint(_impulse, point.worldPoint);
|
||||
}
|
||||
|
||||
/**
|
||||
* 解切向摩擦约束（库仑锥）。
|
||||
* 上限 maxF = μ · 法向冲量；超出则等比缩放两切向冲量。
|
||||
*/
|
||||
export function solveFrictionConstraint(
|
||||
body: ImpulseBody,
|
||||
_manifold: ContactManifold,
|
||||
point: ContactPoint,
|
||||
): void {
|
||||
const mu = body.friction ?? DEFAULT_FRICTION;
|
||||
const maxF = mu * point.normalImpulse;
|
||||
if (maxF <= 1e-8) {
|
||||
// 无法向压力则无摩擦
|
||||
point.tangentImpulse1 = 0;
|
||||
point.tangentImpulse2 = 0;
|
||||
return;
|
||||
}
|
||||
|
||||
body.getVelocityAtPoint(point.worldPoint, _vel);
|
||||
const vt1 = _vel.dot(point.tangent1);
|
||||
const vt2 = _vel.dot(point.tangent2);
|
||||
const mass1 = effectiveMass(body, point.worldPoint, point.tangent1, _r, _gcross, _local, _world, _invQuat);
|
||||
const mass2 = effectiveMass(body, point.worldPoint, point.tangent2, _r, _gcross, _local, _world, _invQuat);
|
||||
|
||||
// 累计切向冲量，使切向相对速度趋近 0
|
||||
const old1 = point.tangentImpulse1;
|
||||
const old2 = point.tangentImpulse2;
|
||||
point.tangentImpulse1 += mass1 > 0 ? -vt1 * mass1 : 0;
|
||||
point.tangentImpulse2 += mass2 > 0 ? -vt2 * mass2 : 0;
|
||||
|
||||
// 钳到摩擦锥：‖λt‖ ≤ μ λn
|
||||
const magSq = point.tangentImpulse1 * point.tangentImpulse1 + point.tangentImpulse2 * point.tangentImpulse2;
|
||||
const maxSq = maxF * maxF;
|
||||
if (magSq > maxSq && magSq > 1e-12) {
|
||||
const scale = maxF / Math.sqrt(magSq);
|
||||
point.tangentImpulse1 *= scale;
|
||||
point.tangentImpulse2 *= scale;
|
||||
}
|
||||
|
||||
// 只施加本轮增量
|
||||
const d1 = point.tangentImpulse1 - old1;
|
||||
const d2 = point.tangentImpulse2 - old2;
|
||||
if (d1 === 0 && d2 === 0) return;
|
||||
_impulse.copy(point.tangent1).multiplyScalar(d1).addScaledVector(point.tangent2, d2);
|
||||
body.applyImpulseAtPoint(_impulse, point.worldPoint);
|
||||
}
|
||||
@@ -0,0 +1,92 @@
|
||||
import * as THREE from "three";
|
||||
import { effectiveMass } from "../../utils/vehiclePhysics/vehicleMath";
|
||||
import { CONTACT_REF_EXTENT } from "../contacts/ContactPoint";
|
||||
import type { ContactManifold } from "../contacts/ContactManifold";
|
||||
import type { ContactPoint } from "../contacts/ContactPoint";
|
||||
import type { ImpulseBody } from "./ImpulseBody";
|
||||
|
||||
/** 下列为 CONTACT_REF_EXTENT 下的基准值；求解时按特征尺寸缩放。 */
|
||||
const PENETRATION_SLOP = 0.003; // 允许的残留穿透，小于此不修正
|
||||
const PENETRATION_BIAS_FACTOR = 0.05; // Baumgarte 系数
|
||||
const MAX_CORRECTION_VELOCITY = 0.04; // 穿透修正速度上限，避免弹飞
|
||||
const SUPPORT_BIAS_Y = 0.5; // 仅对偏竖直的支撑面做法向 bias
|
||||
const RESTITUTION_THRESHOLD = 0.1; // 接近静止时不再加弹性，避免微弹
|
||||
|
||||
const _r = new THREE.Vector3();
|
||||
const _gcross = new THREE.Vector3();
|
||||
const _local = new THREE.Vector3();
|
||||
const _world = new THREE.Vector3();
|
||||
const _invQuat = new THREE.Quaternion();
|
||||
const _vel = new THREE.Vector3();
|
||||
const _impulse = new THREE.Vector3();
|
||||
|
||||
function contactScale(body: ImpulseBody): number {
|
||||
const extent = body.characteristicExtent?.() ?? CONTACT_REF_EXTENT;
|
||||
return Math.max(1e-4, extent) / CONTACT_REF_EXTENT;
|
||||
}
|
||||
|
||||
/**
|
||||
* 准备法向约束的 biasVelocity。
|
||||
* 含弹性目标分离速度，以及支撑面上的穿透位置修正。
|
||||
*/
|
||||
export function prepareNormalConstraint(
|
||||
body: ImpulseBody,
|
||||
manifold: ContactManifold,
|
||||
point: ContactPoint,
|
||||
dt: number,
|
||||
): void {
|
||||
point.biasVelocity = 0;
|
||||
const scale = contactScale(body);
|
||||
body.getVelocityAtPoint(point.worldPoint, _vel);
|
||||
const vn = _vel.dot(manifold.normal);
|
||||
const e = body.restitution ?? 0;
|
||||
if (e > 0 && vn < -RESTITUTION_THRESHOLD * scale) {
|
||||
// 目标分离速度 ≈ -e·vn；并入 bias 后 λ = -(vn - bias)·m ≈ (1+e)|vn|·m
|
||||
point.biasVelocity += -e * vn;
|
||||
}
|
||||
if (dt <= 1e-8) return;
|
||||
// 墙面等非支撑接触不做穿透 bias，减少侧向拉扯
|
||||
if (manifold.normal.y <= SUPPORT_BIAS_Y) return;
|
||||
const error = point.penetration - PENETRATION_SLOP * scale;
|
||||
if (error <= 0) return;
|
||||
point.biasVelocity += Math.min(
|
||||
error * PENETRATION_BIAS_FACTOR / dt,
|
||||
MAX_CORRECTION_VELOCITY * scale,
|
||||
);
|
||||
}
|
||||
|
||||
/** 热启动：施加上一帧累计的法向冲量。 */
|
||||
export function warmStartNormal(
|
||||
body: ImpulseBody,
|
||||
manifold: ContactManifold,
|
||||
point: ContactPoint,
|
||||
): void {
|
||||
if (point.normalImpulse === 0) return;
|
||||
_impulse.copy(manifold.normal).multiplyScalar(point.normalImpulse);
|
||||
body.applyImpulseAtPoint(_impulse, point.worldPoint);
|
||||
}
|
||||
|
||||
/**
|
||||
* 解法向非穿透约束。
|
||||
* 冲量累计且钳制为 ≥ 0（只推开、不拉回）。
|
||||
*/
|
||||
export function solveNormalConstraint(
|
||||
body: ImpulseBody,
|
||||
manifold: ContactManifold,
|
||||
point: ContactPoint,
|
||||
): void {
|
||||
const n = manifold.normal;
|
||||
body.getVelocityAtPoint(point.worldPoint, _vel);
|
||||
const vn = _vel.dot(n);
|
||||
const mass = effectiveMass(body, point.worldPoint, n, _r, _gcross, _local, _world, _invQuat);
|
||||
if (mass <= 0) return;
|
||||
|
||||
// 使 vn 趋向 biasVelocity（弹性 + 穿透修正）
|
||||
const lambda = -(vn - point.biasVelocity) * mass;
|
||||
const old = point.normalImpulse;
|
||||
point.normalImpulse = Math.max(0, old + lambda);
|
||||
const applied = point.normalImpulse - old;
|
||||
if (applied === 0) return;
|
||||
_impulse.copy(n).multiplyScalar(applied);
|
||||
body.applyImpulseAtPoint(_impulse, point.worldPoint);
|
||||
}
|
||||
Reference in new issue
Block a user