Skip to content

Commit

Permalink
Merge pull request #1539 from mrxz/optimize-spring-bones
Browse files Browse the repository at this point in the history 
Optimize spring bones computations
  • Loading branch information
@0b5vr
0b5vr authored Dec 2, 2024
2 parents 3588ce8 + a26974e commit b35bc21
Show file tree
Hide file tree
Showing 10 changed files with 233 additions and 183 deletions.
34 changes: 34 additions & 0 deletions packages/three-vrm-springbone/src/VRMSpringBoneCollider.ts
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,9 +10,43 @@ export class VRMSpringBoneCollider extends THREE.Object3D {
*/
public readonly shape: VRMSpringBoneColliderShape;

/**
* World space matrix for the collider shape used in collision calculations.
*/
public readonly colliderMatrix = new THREE.Matrix4();

public constructor(shape: VRMSpringBoneColliderShape) {
super();

this.shape = shape;
}

public updateWorldMatrix(updateParents: boolean, updateChildren: boolean): void {
super.updateWorldMatrix(updateParents, updateChildren);

updateColliderMatrix(this.colliderMatrix, this.matrixWorld, this.shape.offset);
}
}

/**
* Computes the colliderMatrix based on an offset and a world matrix.
* Equivalent to the following code when matrixWorld is an affine matrix:
* ```js
* out.makeTranslation(offset).premultiply(matrixWorld)
* ```
*
* @param colliderMatrix The target matrix to store the result in.
* @param matrixWorld The world matrix fo the collider object.
* @param offset Optional offset to the collider shape.
*/
function updateColliderMatrix(colliderMatrix: THREE.Matrix4, matrixWorld: THREE.Matrix4, offset?: THREE.Vector3) {
const me = matrixWorld.elements;

colliderMatrix.copy(matrixWorld);

if (offset) {
colliderMatrix.elements[12] = me[0] * offset.x + me[4] * offset.y + me[8] * offset.z + me[12];
colliderMatrix.elements[13] = me[1] * offset.x + me[5] * offset.y + me[9] * offset.z + me[13];
colliderMatrix.elements[14] = me[2] * offset.x + me[6] * offset.y + me[10] * offset.z + me[14];
}
}
5 changes: 5 additions & 0 deletions packages/three-vrm-springbone/src/VRMSpringBoneColliderShape.ts
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,11 @@ export abstract class VRMSpringBoneColliderShape {
*/
public abstract get type(): string;

/**
* The offset to the shape.
*/
public offset?: THREE.Vector3;

/**
* Calculate a distance and a direction from the collider to a target object.
* It's hit if the distance is negative.
Expand Down
17 changes: 9 additions & 8 deletions packages/three-vrm-springbone/src/VRMSpringBoneColliderShapeCapsule.ts
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,8 +44,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
_v3A.setFromMatrixPosition(colliderMatrix); // transformed head
_v3B.subVectors(this.tail, this.offset).applyMatrix4(colliderMatrix); // transformed tail
_v3B.sub(_v3A); // from head to tail
const lengthSqCapsule = _v3B.lengthSq();

Expand All @@ -64,13 +64,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;
Expand Down
2 changes: 1 addition & 1 deletion packages/three-vrm-springbone/src/VRMSpringBoneColliderShapePlane.ts
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
18 changes: 10 additions & 8 deletions packages/three-vrm-springbone/src/VRMSpringBoneColliderShapeSphere.ts
View file
Original file line number Diff line number Diff line change
@@ -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';
Expand Down Expand Up @@ -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;
Expand Down
132 changes: 55 additions & 77 deletions packages/three-vrm-springbone/src/VRMSpringBoneJoint.ts
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,4 @@
import * as THREE from 'three';
import { mat4InvertCompat } from './utils/mat4InvertCompat';
import { Matrix4InverseCache } from './utils/Matrix4InverseCache';
import type { VRMSpringBoneColliderGroup } from './VRMSpringBoneColliderGroup';
import type { VRMSpringBoneJointSettings } from './VRMSpringBoneJointSettings';
Expand All @@ -13,26 +12,18 @@ const IDENTITY_MATRIX4 = new THREE.Matrix4();
// 計算中の一時保存用変数（一度インスタンスを作ったらあとは使い回す）
const _v3A = new THREE.Vector3();
const _v3B = new THREE.Vector3();
const _v3C = new THREE.Vector3();

/**
* A temporary variable which is used in `update`
*/
const _worldSpacePosition = new THREE.Vector3();

/**
* A temporary variable which is used in `update`
*/
const _centerSpacePosition = new THREE.Vector3();

/**
* A temporary variable which is used in `update`
*/
const _nextTail = new THREE.Vector3();

const _quatA = new THREE.Quaternion();
const _matA = new THREE.Matrix4();
const _matB = new THREE.Matrix4();

/**
* A class represents a single joint of a spring bone.
Expand Down Expand Up @@ -83,6 +74,26 @@ export class VRMSpringBoneJoint {
*/
private _worldSpaceBoneLength = 0.0;

/**
* Set of dependencies that need to be updated before this joint.
*/
public get dependencies(): Set<THREE.Object3D> {
const set = new Set<THREE.Object3D>();

const parent = this.bone.parent;
if (parent) {
set.add(parent);
}

for (let cg = 0; cg < this.colliderGroups.length; cg++) {
for (let c = 0; c < this.colliderGroups[cg].colliders.length; c++) {
set.add(this.colliderGroups[cg].colliders[c]);
}
}

return set;
}

/**
* This springbone will be calculated based on the space relative from this object.
* If this is `null`, springbone will be calculated in world space.
Expand Down Expand Up @@ -184,7 +195,7 @@ export class VRMSpringBoneJoint {
}

// copy the child position to tails
const matrixWorldToCenter = this._getMatrixWorldToCenter(_matA);
const matrixWorldToCenter = this._getMatrixWorldToCenter();
this.bone.localToWorld(this._currentTail.copy(this._initialLocalChildPosition)).applyMatrix4(matrixWorldToCenter);
this._prevTail.copy(this._currentTail);

Expand All @@ -204,7 +215,7 @@ export class VRMSpringBoneJoint {
this.bone.matrixWorld.multiplyMatrices(this._parentMatrixWorld, this.bone.matrix);

// Apply updated position to tail states
const matrixWorldToCenter = this._getMatrixWorldToCenter(_matA);
const matrixWorldToCenter = this._getMatrixWorldToCenter();
this.bone.localToWorld(this._currentTail.copy(this._initialLocalChildPosition)).applyMatrix4(matrixWorldToCenter);
this._prevTail.copy(this._currentTail);
}
Expand All @@ -221,64 +232,42 @@ export class VRMSpringBoneJoint {
// Update the _worldSpaceBoneLength
this._calcWorldSpaceBoneLength();

// Get bone position in center space
_worldSpacePosition.setFromMatrixPosition(this.bone.matrixWorld);
let matrixWorldToCenter = this._getMatrixWorldToCenter(_matA);
_centerSpacePosition.copy(_worldSpacePosition).applyMatrix4(matrixWorldToCenter);
const quatWorldToCenter = _quatA.setFromRotationMatrix(matrixWorldToCenter);

// Get parent matrix in center space
const centerSpaceParentMatrix = _matB.copy(matrixWorldToCenter).multiply(this._parentMatrixWorld);

// Get boneAxis in center space
const centerSpaceBoneAxis = _v3B
// Get boneAxis in world space
const worldSpaceBoneAxis = _v3B
.copy(this._boneAxis)
.applyMatrix4(this._initialLocalMatrix)
.applyMatrix4(centerSpaceParentMatrix)
.sub(_centerSpacePosition)
.normalize();

// gravity in center space
const centerSpaceGravity = _v3C.copy(this.settings.gravityDir).applyQuaternion(quatWorldToCenter).normalize();

const matrixCenterToWorld = this._getMatrixCenterToWorld(_matA);
.transformDirection(this._initialLocalMatrix)
.transformDirection(this._parentMatrixWorld);

// verlet積分で次の位置を計算
_nextTail
// Determine inertia in center space
.copy(this._currentTail)
.add(
_v3A
.copy(this._currentTail)
.sub(this._prevTail)
.multiplyScalar(1 - this.settings.dragForce),
) // 前フレームの移動を継続する(減衰もあるよ)
.add(_v3A.copy(centerSpaceBoneAxis).multiplyScalar(this.settings.stiffness * delta)) // 親の回転による子ボーンの移動目標
.add(_v3A.copy(centerSpaceGravity).multiplyScalar(this.settings.gravityPower * delta)) // 外力による移動量
.applyMatrix4(matrixCenterToWorld); // tailをworld spaceに戻す
.add(_v3A.subVectors(this._currentTail, this._prevTail).multiplyScalar(1 - this.settings.dragForce)) // 前フレームの移動を継続する(減衰もあるよ)
// Convert center space to world space
.applyMatrix4(this._getMatrixCenterToWorld()) // tailをworld spaceに戻す
// Apply stiffness and gravity in world space
.addScaledVector(worldSpaceBoneAxis, this.settings.stiffness * delta) // 親の回転による子ボーンの移動目標
.addScaledVector(this.settings.gravityDir, this.settings.gravityPower * delta); // 外力による移動量

// normalize bone length
_worldSpacePosition.setFromMatrixPosition(this.bone.matrixWorld);
_nextTail.sub(_worldSpacePosition).normalize().multiplyScalar(this._worldSpaceBoneLength).add(_worldSpacePosition);

// Collisionで移動
this._collision(_nextTail);

// update prevTail and currentTail
matrixWorldToCenter = this._getMatrixWorldToCenter(_matA);

this._prevTail.copy(this._currentTail);
this._currentTail.copy(_v3A.copy(_nextTail).applyMatrix4(matrixWorldToCenter));
this._currentTail.copy(_nextTail).applyMatrix4(this._getMatrixWorldToCenter());

// Apply rotation, convert vector3 thing into actual quaternion
// Original UniVRM is doing center unit calculus at here but we're gonna do this on local unit
const worldSpaceInitialMatrixInv = mat4InvertCompat(
_matA.copy(this._parentMatrixWorld).multiply(this._initialLocalMatrix),
);
const applyRotation = _quatA.setFromUnitVectors(
this._boneAxis,
_v3A.copy(_nextTail).applyMatrix4(worldSpaceInitialMatrixInv).normalize(),
);

this.bone.quaternion.copy(this._initialLocalRotation).multiply(applyRotation);
const worldSpaceInitialMatrixInv = _matA
.multiplyMatrices(this._parentMatrixWorld, this._initialLocalMatrix)
.invert();
this.bone.quaternion
.setFromUnitVectors(this._boneAxis, _v3A.copy(_nextTail).applyMatrix4(worldSpaceInitialMatrixInv).normalize())
.premultiply(this._initialLocalRotation);

// We need to update its matrixWorld manually, since we tweaked the bone by our hand
this.bone.updateMatrix();
Expand All @@ -291,19 +280,22 @@ export class VRMSpringBoneJoint {
* @param tail The tail you want to process
*/
private _collision(tail: THREE.Vector3): void {
this.colliderGroups.forEach((colliderGroup) => {
colliderGroup.colliders.forEach((collider) => {
const dist = collider.shape.calculateCollision(collider.matrixWorld, tail, this.settings.hitRadius, _v3A);
for (let cg = 0; cg < this.colliderGroups.length; cg++) {
for (let c = 0; c < this.colliderGroups[cg].colliders.length; c++) {
const collider = this.colliderGroups[cg].colliders[c];
const dist = collider.shape.calculateCollision(collider.colliderMatrix, tail, this.settings.hitRadius, _v3A);

if (dist < 0.0) {
// hit
tail.add(_v3A.multiplyScalar(-dist));
tail.addScaledVector(_v3A, -dist);

// normalize bone length
tail.sub(_worldSpacePosition).normalize().multiplyScalar(this._worldSpaceBoneLength).add(_worldSpacePosition);
tail.sub(_worldSpacePosition);
const length = tail.length();
tail.multiplyScalar(this._worldSpaceBoneLength / length).add(_worldSpacePosition);
}
});
});
}
}
}

/**
Expand All @@ -325,29 +317,15 @@ export class VRMSpringBoneJoint {

/**
* Create a matrix that converts center space into world space.
* @param target Target matrix
*/
private _getMatrixCenterToWorld(target: THREE.Matrix4): THREE.Matrix4 {
if (this._center) {
target.copy(this._center.matrixWorld);
} else {
target.identity();
}

return target;
private _getMatrixCenterToWorld(): THREE.Matrix4 {
return this._center ? this._center.matrixWorld : IDENTITY_MATRIX4;
}

/**
* Create a matrix that converts world space into center space.
* @param target Target matrix
*/
private _getMatrixWorldToCenter(target: THREE.Matrix4): THREE.Matrix4 {
if (this._center) {
target.copy((this._center.userData.inverseCacheProxy as Matrix4InverseCache).inverse);
} else {
target.identity();
}

return target;
private _getMatrixWorldToCenter(): THREE.Matrix4 {
return this._center ? (this._center.userData.inverseCacheProxy as Matrix4InverseCache).inverse : IDENTITY_MATRIX4;
}
}
Loading

0 comments on commit b35bc21

Please sign in to comment.