diff --git a/fission/src/mirabuf/MirabufSceneObject.ts b/fission/src/mirabuf/MirabufSceneObject.ts index 28dcc298e..728ef4c92 100644 --- a/fission/src/mirabuf/MirabufSceneObject.ts +++ b/fission/src/mirabuf/MirabufSceneObject.ts @@ -21,6 +21,7 @@ import { SceneOverlayTag } from "@/ui/components/SceneOverlayEvents" import { ProgressHandle } from "@/ui/components/ProgressNotificationData" import SynthesisBrain from "@/systems/simulation/synthesis_brain/SynthesisBrain" import GizmoSceneObject from "@/systems/scene/GizmoSceneObject" +import { threeMatrix4ToString } from "@/util/debug/DebugPrint" const DEBUG_BODIES = false @@ -183,33 +184,7 @@ class MirabufSceneObject extends SceneObject { this.Eject() } - /** Updating the position of all mirabuf nodes */ - this._mirabufInstance.parser.rigidNodes.forEach(rn => { - if (!this._mirabufInstance.meshes.size) return // if this.dispose() has been ran then return - const body = World.PhysicsSystem.GetBody(this._mechanism.GetBodyByNodeId(rn.id)!) - const transform = JoltMat44_ThreeMatrix4(body.GetWorldTransform()) - this.UpdateNodeParts(rn, transform) - - if (isNaN(body.GetPosition().GetX())) { - const vel = body.GetLinearVelocity() - const pos = body.GetPosition() - console.warn( - `Invalid Position.\nPosition => ${pos.GetX()}, ${pos.GetY()}, ${pos.GetZ()}\nVelocity => ${vel.GetX()}, ${vel.GetY()}, ${vel.GetZ()}` - ) - } - // console.debug(`POSITION: ${body.GetPosition().GetX()}, ${body.GetPosition().GetY()}, ${body.GetPosition().GetZ()}`) - - if (this._debugBodies) { - const { colliderMesh, comMesh } = this._debugBodies.get(rn.id)! - colliderMesh.position.setFromMatrixPosition(transform) - colliderMesh.rotation.setFromRotationMatrix(transform) - - const comTransform = JoltMat44_ThreeMatrix4(body.GetCenterOfMassTransform()) - - comMesh.position.setFromMatrixPosition(comTransform) - comMesh.rotation.setFromRotationMatrix(comTransform) - } - }) + this.UpdateMeshTransforms() this.UpdateBatches() this.UpdateNameTag() @@ -293,6 +268,37 @@ class MirabufSceneObject extends SceneObject { return mesh } + /** + * Matches mesh transforms to their Jolt counterparts. + */ + public UpdateMeshTransforms() { + this._mirabufInstance.parser.rigidNodes.forEach(rn => { + if (!this._mirabufInstance.meshes.size) return // if this.dispose() has been ran then return + const body = World.PhysicsSystem.GetBody(this._mechanism.GetBodyByNodeId(rn.id)!) + const transform = JoltMat44_ThreeMatrix4(body.GetWorldTransform()) + this.UpdateNodeParts(rn, transform) + + if (isNaN(body.GetPosition().GetX())) { + const vel = body.GetLinearVelocity() + const pos = body.GetPosition() + console.warn( + `Invalid Position.\nPosition => ${pos.GetX()}, ${pos.GetY()}, ${pos.GetZ()}\nVelocity => ${vel.GetX()}, ${vel.GetY()}, ${vel.GetZ()}` + ) + } + + if (this._debugBodies) { + const { colliderMesh, comMesh } = this._debugBodies.get(rn.id)! + colliderMesh.position.setFromMatrixPosition(transform) + colliderMesh.rotation.setFromRotationMatrix(transform) + + const comTransform = JoltMat44_ThreeMatrix4(body.GetCenterOfMassTransform()) + + comMesh.position.setFromMatrixPosition(comTransform) + comMesh.rotation.setFromRotationMatrix(comTransform) + } + }) + } + public UpdateNodeParts(rn: RigidNodeReadOnly, transform: THREE.Matrix4) { rn.parts.forEach(part => { const partTransform = this._mirabufInstance.parser.globalTransforms @@ -396,7 +402,18 @@ class MirabufSceneObject extends SceneObject { * @param gizmo Gizmo attached to the mirabuf object */ public PostGizmoCreation(gizmo: GizmoSceneObject) { - // TODO: Move to the center of the bot/field + const jRootId = this.GetRootNodeId() + if (!jRootId) { + console.error("No root node found.") + return + } + + const jBody = World.PhysicsSystem.GetBody(jRootId) + const comTransform = JoltMat44_ThreeMatrix4(jBody.GetCenterOfMassTransform()) + gizmo.UpdateGizmoObjectPositionAndRotation(comTransform) + + console.debug(`Source:\n${threeMatrix4ToString(comTransform)}`) + console.debug(`New:\n${threeMatrix4ToString(gizmo.obj.matrix)}`) } private getPreferences(): void { diff --git a/fission/src/systems/physics/PhysicsSystem.ts b/fission/src/systems/physics/PhysicsSystem.ts index f72f1c955..e688a3a7f 100644 --- a/fission/src/systems/physics/PhysicsSystem.ts +++ b/fission/src/systems/physics/PhysicsSystem.ts @@ -1080,6 +1080,19 @@ class PhysicsSystem extends WorldSystem { ) } + public SetBodyPositionAndRotation(id: Jolt.BodyID, position: Jolt.Vec3, rotation: Jolt.Quat, activate: boolean = true): void { + if (!this.IsBodyAdded(id)) { + return + } + + this._joltBodyInterface.SetPositionAndRotation( + id, + position, + rotation, + activate ? JOLT.EActivation_Activate : JOLT.EActivation_DontActivate + ) + } + /** * Exposes SetShape method on the _joltBodyInterface * Sets the shape of the body diff --git a/fission/src/systems/scene/GizmoSceneObject.ts b/fission/src/systems/scene/GizmoSceneObject.ts index 3f9fc1c6b..c98e56fa8 100644 --- a/fission/src/systems/scene/GizmoSceneObject.ts +++ b/fission/src/systems/scene/GizmoSceneObject.ts @@ -5,15 +5,18 @@ import InputSystem from "../input/InputSystem" import World from "../World" import MirabufSceneObject from "@/mirabuf/MirabufSceneObject" import { Object3D, PerspectiveCamera } from "three" -import { ThreeMatrix4_JoltMat44, ThreeQuaternion_JoltQuat } from "@/util/TypeConversions" -import { RigidNodeReadOnly } from "@/mirabuf/MirabufParser" +import { ThreeQuaternion_JoltQuat, JoltMat44_ThreeMatrix4, ThreeVector3_JoltVec3 } from "@/util/TypeConversions" +import { RigidNodeId } from "@/mirabuf/MirabufParser" +import { threeMatrix4ToString } from "@/util/debug/DebugPrint" export type GizmoMode = "translate" | "rotate" | "scale" class GizmoSceneObject extends SceneObject { private _gizmo: TransformControls private _obj: Object3D + private _parentObject: MirabufSceneObject | undefined + private _relativeTransformations?: Map private _mainCamera: PerspectiveCamera @@ -43,7 +46,8 @@ class GizmoSceneObject extends SceneObject { mode: GizmoMode, size: number, obj?: THREE.Mesh, - parentObject?: MirabufSceneObject + parentObject?: MirabufSceneObject, + postGizmoCreation?: (gizmo: GizmoSceneObject) => void, ) { super() @@ -57,6 +61,26 @@ class GizmoSceneObject extends SceneObject { this._gizmo.setMode(mode) World.SceneRenderer.RegisterGizmoSceneObject(this) + + postGizmoCreation?.(this) + + if (this._parentObject) { + this._relativeTransformations = new Map() + const c = this._obj.matrix.clone() + console.debug(`Clone:\n${threeMatrix4ToString(c)}`) + + const gizmoTransformInv = c.invert() + + /** Due to the limited math functionality exposed to JS for Jolt, we need everything in ThreeJS. */ + this._parentObject.mirabufInstance.parser.rigidNodes.forEach(rn => { + const jBodyId = this._parentObject!.mechanism.GetBodyByNodeId(rn.id) + if (!jBodyId) return + + const worldTransform = JoltMat44_ThreeMatrix4(World.PhysicsSystem.GetBody(jBodyId).GetWorldTransform()) + const relativeTransform = worldTransform.premultiply(gizmoTransformInv) + this._relativeTransformations!.set(rn.id, relativeTransform) + }) + } } public Setup(): void { @@ -131,9 +155,9 @@ class GizmoSceneObject extends SceneObject { this._parentObject.DisablePhysics() if (this.isDragging) { this._parentObject.mirabufInstance.parser.rigidNodes.forEach(rn => { - this.UpdateBodyPositionAndRotation(rn) - this._parentObject?.UpdateNodeParts(rn, this.obj.matrix) + this.UpdateNodeTransform(rn.id) }) + this._parentObject.UpdateMeshTransforms() } } } @@ -151,22 +175,35 @@ class GizmoSceneObject extends SceneObject { } /** updates body position and rotation for each body from the parent mirabuf */ - public UpdateBodyPositionAndRotation(rn: RigidNodeReadOnly) { - if (!this._parentObject) return - World.PhysicsSystem.SetBodyPosition( - this._parentObject.mechanism.GetBodyByNodeId(rn.id)!, - ThreeMatrix4_JoltMat44(this._obj.matrix).GetTranslation() - ) - World.PhysicsSystem.SetBodyRotation( - this._parentObject.mechanism.GetBodyByNodeId(rn.id)!, - ThreeQuaternion_JoltQuat(this._obj.quaternion) + public UpdateNodeTransform(rnId: RigidNodeId) { + if (!this._parentObject || !this._relativeTransformations || !this._relativeTransformations.has(rnId)) return + + const jBodyId = this._parentObject.mechanism.GetBodyByNodeId(rnId) + if (!jBodyId) return + + const relativeTransform = this._relativeTransformations.get(rnId)! + const worldTransform = relativeTransform.clone().premultiply(this._obj.matrix) + const position = new THREE.Vector3(0,0,0) + const rotation = new THREE.Quaternion(0,0,0,1) + worldTransform.decompose(position, rotation, new THREE.Vector3(1,1,1)) + + World.PhysicsSystem.SetBodyPositionAndRotation( + jBodyId, + ThreeVector3_JoltVec3(position), + ThreeQuaternion_JoltQuat(rotation), ) } /** */ public UpdateGizmoObjectPositionAndRotation(gizmoTransformation: THREE.Matrix4) { + const position = new THREE.Vector3(0,0,0) + const rotation = new THREE.Quaternion(0,0,0,1) + const scale = new THREE.Vector3(1,1,1) + gizmoTransformation.decompose(position, rotation, scale) + this._obj.matrix.compose(position, rotation, scale) + this._obj.position.setFromMatrixPosition(gizmoTransformation) - this._obj.quaternion.setFromRotationMatrix(gizmoTransformation) + this._obj.rotation.setFromRotationMatrix(gizmoTransformation) } /** @return true if gizmo is attached to mirabufSceneObject */ diff --git a/fission/src/ui/components/TransformGizmoControl.tsx b/fission/src/ui/components/TransformGizmoControl.tsx index c28adc7e2..4360d217f 100644 --- a/fission/src/ui/components/TransformGizmoControl.tsx +++ b/fission/src/ui/components/TransformGizmoControl.tsx @@ -36,12 +36,13 @@ function TransformGizmoControl({ "translate", size, defaultMesh, - parent + parent, + (gizmo: GizmoSceneObject) => { + parent?.PostGizmoCreation(gizmo) + postGizmoCreation?.(gizmo) + } ) - parent?.PostGizmoCreation(gizmo) - postGizmoCreation?.(gizmo) - if (gizmoRef) gizmoRef.current = gizmo setGizmo(gizmo)