diff --git a/packages/three-vrm-springbone/src/VRMSpringBoneCollider.ts b/packages/three-vrm-springbone/src/VRMSpringBoneCollider.ts index 3f576afa2..4a77d32aa 100644 --- a/packages/three-vrm-springbone/src/VRMSpringBoneCollider.ts +++ b/packages/three-vrm-springbone/src/VRMSpringBoneCollider.ts @@ -14,5 +14,8 @@ export class VRMSpringBoneCollider extends THREE.Object3D { super(); this.shape = shape; + if ('offset' in shape && (shape.offset as THREE.Vector3).isVector3) { + this.position.copy(shape.offset as THREE.Vector3); + } } } diff --git a/packages/three-vrm-springbone/src/VRMSpringBoneColliderShapeCapsule.ts b/packages/three-vrm-springbone/src/VRMSpringBoneColliderShapeCapsule.ts index 1d6aa51a0..6bfb7f4b0 100644 --- a/packages/three-vrm-springbone/src/VRMSpringBoneColliderShapeCapsule.ts +++ b/packages/three-vrm-springbone/src/VRMSpringBoneColliderShapeCapsule.ts @@ -29,11 +29,17 @@ export class VRMSpringBoneColliderShapeCapsule extends VRMSpringBoneColliderShap */ public inside: boolean; + /** + * Vector from head to tail in colliders local space. + */ + private _headToTail: THREE.Vector3; + public constructor(params?: { radius?: number; offset?: THREE.Vector3; tail?: THREE.Vector3; inside?: boolean }) { super(); this.offset = params?.offset ?? new THREE.Vector3(0.0, 0.0, 0.0); this.tail = params?.tail ?? new THREE.Vector3(0.0, 0.0, 0.0); + this._headToTail = this.tail.clone().sub(this.offset); this.radius = params?.radius ?? 0.0; this.inside = params?.inside ?? false; } @@ -44,9 +50,8 @@ export class VRMSpringBoneColliderShapeCapsule extends VRMSpringBoneColliderShap objectRadius: number, target: THREE.Vector3, ): number { - _v3A.copy(this.offset).applyMatrix4(colliderMatrix); // transformed head - _v3B.copy(this.tail).applyMatrix4(colliderMatrix); // transformed tail - _v3B.sub(_v3A); // from head to tail + _v3A.setFromMatrixPosition(colliderMatrix); // transformed head + _v3B.copy(this._headToTail).transformDirection(colliderMatrix).multiplyScalar(this._headToTail.length()); // transformed tail const lengthSqCapsule = _v3B.lengthSq(); target.copy(objectPosition).sub(_v3A); // from head to object @@ -64,13 +69,14 @@ export class VRMSpringBoneColliderShapeCapsule extends VRMSpringBoneColliderShap target.sub(_v3B); // from the shaft point to object } - const distance = this.inside - ? this.radius - objectRadius - target.length() - : target.length() - objectRadius - this.radius; + const length = target.length(); + const distance = this.inside ? this.radius - objectRadius - length : length - objectRadius - this.radius; - target.normalize(); // convert the delta to the direction - if (this.inside) { - target.negate(); // if inside, reverse the direction + if (distance < 0) { + target.multiplyScalar(1 / length); // convert the delta to the direction + if (this.inside) { + target.negate(); // if inside, reverse the direction + } } return distance; diff --git a/packages/three-vrm-springbone/src/VRMSpringBoneColliderShapePlane.ts b/packages/three-vrm-springbone/src/VRMSpringBoneColliderShapePlane.ts index 087d1fd26..b8fd1002c 100644 --- a/packages/three-vrm-springbone/src/VRMSpringBoneColliderShapePlane.ts +++ b/packages/three-vrm-springbone/src/VRMSpringBoneColliderShapePlane.ts @@ -32,7 +32,7 @@ export class VRMSpringBoneColliderShapePlane extends VRMSpringBoneColliderShape objectRadius: number, target: THREE.Vector3, ): number { - target.copy(this.offset).applyMatrix4(colliderMatrix); // transformed offset + target.setFromMatrixPosition(colliderMatrix); // transformed offset target.negate().add(objectPosition); // a vector from collider center to object position _mat3A.getNormalMatrix(colliderMatrix); // convert the collider matrix to the normal matrix diff --git a/packages/three-vrm-springbone/src/VRMSpringBoneColliderShapeSphere.ts b/packages/three-vrm-springbone/src/VRMSpringBoneColliderShapeSphere.ts index 95aac74d1..942e973ae 100644 --- a/packages/three-vrm-springbone/src/VRMSpringBoneColliderShapeSphere.ts +++ b/packages/three-vrm-springbone/src/VRMSpringBoneColliderShapeSphere.ts @@ -1,6 +1,8 @@ import * as THREE from 'three'; import { VRMSpringBoneColliderShape } from './VRMSpringBoneColliderShape'; +const _v3A = new THREE.Vector3(); + export class VRMSpringBoneColliderShapeSphere extends VRMSpringBoneColliderShape { public get type(): 'sphere' { return 'sphere'; @@ -35,16 +37,16 @@ export class VRMSpringBoneColliderShapeSphere extends VRMSpringBoneColliderShape objectRadius: number, target: THREE.Vector3, ): number { - target.copy(this.offset).applyMatrix4(colliderMatrix); // transformed offset - target.negate().add(objectPosition); // a vector from collider center to object position + target.subVectors(objectPosition, _v3A.setFromMatrixPosition(colliderMatrix)); - const distance = this.inside - ? this.radius - objectRadius - target.length() - : target.length() - objectRadius - this.radius; + const length = target.length(); + const distance = this.inside ? this.radius - objectRadius - length : length - objectRadius - this.radius; - target.normalize(); // convert the delta to the direction - if (this.inside) { - target.negate(); // if inside, reverse the direction + if (distance < 0) { + target.multiplyScalar(1 / length); // convert the delta to the direction + if (this.inside) { + target.negate(); // if inside, reverse the direction + } } return distance; diff --git a/packages/three-vrm-springbone/src/tests/VRMSpringBoneColliderShapeCapsule.test.ts b/packages/three-vrm-springbone/src/tests/VRMSpringBoneColliderShapeCapsule.test.ts index 92329f568..eb4fe22e4 100644 --- a/packages/three-vrm-springbone/src/tests/VRMSpringBoneColliderShapeCapsule.test.ts +++ b/packages/three-vrm-springbone/src/tests/VRMSpringBoneColliderShapeCapsule.test.ts @@ -75,7 +75,7 @@ describe('VRMSpringBoneColliderShapeCapsule', () => { }); it('must calculate a collision properly, object is near the head', () => { - const colliderMatrix = new THREE.Matrix4(); + const colliderMatrix = new THREE.Matrix4().makeTranslation(shape.offset); const objectPosition = new THREE.Vector3(-2.0, 0.0, 1.0); const objectRadius = 1.0; @@ -87,19 +87,19 @@ describe('VRMSpringBoneColliderShapeCapsule', () => { }); it('must calculate a collision properly, object is near the tail', () => { - const colliderMatrix = new THREE.Matrix4(); + const colliderMatrix = new THREE.Matrix4().makeTranslation(shape.offset); const objectPosition = new THREE.Vector3(3.0, 0.0, 0.0); - const objectRadius = 1.0; + const objectRadius = 2.0; const dir = new THREE.Vector3(); const distSq = shape.calculateCollision(colliderMatrix, objectPosition, objectRadius, dir); - expect(distSq).toBeCloseTo(0.44949); // sqrt(6) - 2 + expect(distSq).toBeCloseTo(-0.55051); // sqrt(6) - 3 expect(dir).toBeCloseToVector3(new THREE.Vector3(2.0, -1.0, -1.0).normalize()); }); it('must calculate a collision properly, object is between two ends', () => { - const colliderMatrix = new THREE.Matrix4(); + const colliderMatrix = new THREE.Matrix4().makeTranslation(shape.offset); const objectPosition = new THREE.Vector3(0.0, 0.0, 0.0); const objectRadius = 1.0; diff --git a/packages/three-vrm-springbone/src/tests/VRMSpringBoneColliderShapeSphere.test.ts b/packages/three-vrm-springbone/src/tests/VRMSpringBoneColliderShapeSphere.test.ts index 9c19ce3a6..fddd977ad 100644 --- a/packages/three-vrm-springbone/src/tests/VRMSpringBoneColliderShapeSphere.test.ts +++ b/packages/three-vrm-springbone/src/tests/VRMSpringBoneColliderShapeSphere.test.ts @@ -64,7 +64,9 @@ describe('VRMSpringBoneColliderShapeSphere', () => { offset: new THREE.Vector3(0.0, 0.0, -1.0), }); - const colliderMatrix = new THREE.Matrix4().makeTranslation(1.0, 0.0, 0.0); + const colliderMatrix = new THREE.Matrix4() + .makeTranslation(1.0, 0.0, 0.0) + .multiply(new THREE.Matrix4().makeTranslation(shape.offset)); const objectPosition = new THREE.Vector3(2.0, 1.0, 0.0); const objectRadius = 1.0; @@ -81,7 +83,9 @@ describe('VRMSpringBoneColliderShapeSphere', () => { offset: new THREE.Vector3(0.0, 1.0, 1.0), }); - const colliderMatrix = new THREE.Matrix4().makeRotationX(-0.5 * Math.PI); + const colliderMatrix = new THREE.Matrix4() + .makeRotationX(-0.5 * Math.PI) + .multiply(new THREE.Matrix4().makeTranslation(shape.offset)); const objectPosition = new THREE.Vector3(-1.0, 1.0, -1.0); const objectRadius = 1.0;