Skip to content

Commit

Permalink
fix: Gizmo centers on selected assembly.
Browse files Browse the repository at this point in the history
  • Loading branch information
@HunterBarclay
HunterBarclay committed Sep 22, 2024
1 parent 38495f7 commit b2b648a
Show file tree
Hide file tree
Showing 4 changed files with 115 additions and 47 deletions.
73 changes: 45 additions & 28 deletions fission/src/mirabuf/MirabufSceneObject.ts
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down Expand Up @@ -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()
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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 {
Expand Down
13 changes: 13 additions & 0 deletions fission/src/systems/physics/PhysicsSystem.ts
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
67 changes: 52 additions & 15 deletions fission/src/systems/scene/GizmoSceneObject.ts
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<RigidNodeId, THREE.Matrix4>

private _mainCamera: PerspectiveCamera

Expand Down Expand Up @@ -43,7 +46,8 @@ class GizmoSceneObject extends SceneObject {
mode: GizmoMode,
size: number,
obj?: THREE.Mesh,
parentObject?: MirabufSceneObject
parentObject?: MirabufSceneObject,
postGizmoCreation?: (gizmo: GizmoSceneObject) => void,
) {
super()

Expand All @@ -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<RigidNodeId, THREE.Matrix4>()
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 {
Expand Down Expand Up @@ -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()
}
}
}
Expand All @@ -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 */
Expand Down
9 changes: 5 additions & 4 deletions fission/src/ui/components/TransformGizmoControl.tsx
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down

0 comments on commit b2b648a

Please sign in to comment.