Fix detected planes

packages/engine/src/transform/systems/TransformSystem.ts

@@ -46,7 +46,8 @@ import {
 
 const transformQuery = defineQuery([TransformComponent])
 const nonDynamicLocalTransformQuery = defineQuery([LocalTransformComponent, Not(RigidBodyDynamicTagComponent)])
-const rigidbodyTransformQuery = defineQuery([TransformComponent, RigidBodyComponent, Not(RigidBodyFixedTagComponent)])
+const rigidbodyTransformQuery = defineQuery([TransformComponent, RigidBodyComponent])
+const fixedRigidBodyQuery = defineQuery([TransformComponent, RigidBodyComponent, RigidBodyFixedTagComponent])
 const groupQuery = defineQuery([GroupComponent, TransformComponent])
 
 const staticBoundingBoxQuery = defineQuery([GroupComponent, BoundingBoxComponent])
@@ -122,6 +123,19 @@ export const lerpTransformFromRigidbody = (entity: Entity, alpha: number) => {
   Engine.instance.currentWorld.dirtyTransforms[entity] = true
 }
 
+export const copyTransformToRigidBody = (entity: Entity) => {
+  RigidBodyComponent.position.x[entity] = TransformComponent.position.x[entity]
+  RigidBodyComponent.position.y[entity] = TransformComponent.position.y[entity]
+  RigidBodyComponent.position.z[entity] = TransformComponent.position.z[entity]
+  RigidBodyComponent.rotation.x[entity] = TransformComponent.rotation.x[entity]
+  RigidBodyComponent.rotation.y[entity] = TransformComponent.rotation.y[entity]
+  RigidBodyComponent.rotation.z[entity] = TransformComponent.rotation.z[entity]
+  RigidBodyComponent.rotation.w[entity] = TransformComponent.rotation.w[entity]
+  const rigidbody = getComponent(entity, RigidBodyComponent)
+  rigidbody.body.setTranslation(rigidbody.position, false)
+  rigidbody.body.setRotation(rigidbody.rotation, false)
+}
+
 const updateTransformFromLocalTransform = (entity: Entity) => {
   const localTransform = getOptionalComponent(entity, LocalTransformComponent)
   const isDynamicRigidbody = hasComponent(entity, RigidBodyDynamicTagComponent)
@@ -309,10 +323,12 @@ export default async function TransformSystem(world: World) {
     const dirtyNonDynamicLocalTransformEntities = nonDynamicLocalTransformQuery().filter(isDirty)
     const dirtySortedTransformEntities = sortedTransformEntities.filter(isDirty)
     const dirtyGroupEntities = groupQuery().filter(isDirty)
+    const dirtyFixedRigidbodyEntities = fixedRigidBodyQuery().filter(isDirty)
 
     for (const entity of dirtyNonDynamicLocalTransformEntities) computeLocalTransformMatrix(entity)
     for (const entity of dirtySortedTransformEntities) computeTransformMatrix(entity, world)
 
+    for (const entity of dirtyFixedRigidbodyEntities) copyTransformToRigidBody(entity)
     for (const entity of dirtyGroupEntities) updateGroupChildren(entity)
 
     if (!xrFrame) {

packages/engine/src/xr/XRDetectedPlanesSystem.ts

@@ -59,10 +59,6 @@ export const createGeometryFromPolygon = (plane: XRPlane) => {
 let planeId = 0
 
 export const foundPlane = (frame: XRFrame & DetectedPlanesType, world: World, plane: XRPlane) => {
-  const referenceSpace = ReferenceSpace.localFloor!
-
-  const planePose = frame.getPose(plane.planeSpace, referenceSpace)!
-
   const geometry = new BufferGeometry()
 
   const entity = createEntity()
@@ -71,10 +67,6 @@ export const foundPlane = (frame: XRFrame & DetectedPlanesType, world: World, pl
   setComponent(entity, XRPlaneComponent)
   setComponent(entity, NameComponent, 'plane-' + planeId++)
 
-  const transform = getComponent(entity, LocalTransformComponent)
-  transform.matrix.fromArray(planePose.transform.matrix)
-  transform.matrix.decompose(transform.position, transform.rotation, transform.scale)
-
   const shadowMat = new ShadowMaterial({ opacity: 0.5, color: 0x0a0a0a })
   shadowMat.polygonOffset = true
   shadowMat.polygonOffsetFactor = -0.01
@@ -147,6 +139,19 @@ export default async function XRDetectedPlanesSystem(world: World) {
         getComponentState(entity, XRPlaneComponent).geometry.set(geometry)
       }
     }
+
+    if (ReferenceSpace.localFloor) {
+      for (const [plane, entity] of detectedPlanesMap) {
+        const planePose = frame.getPose(plane.planeSpace, ReferenceSpace.localFloor)!
+        LocalTransformComponent.position.x[entity] = planePose.transform.position.x
+        LocalTransformComponent.position.y[entity] = planePose.transform.position.y
+        LocalTransformComponent.position.z[entity] = planePose.transform.position.z
+        LocalTransformComponent.rotation.x[entity] = planePose.transform.orientation.x
+        LocalTransformComponent.rotation.y[entity] = planePose.transform.orientation.y
+        LocalTransformComponent.rotation.z[entity] = planePose.transform.orientation.z
+        LocalTransformComponent.rotation.w[entity] = planePose.transform.orientation.w
+      }
+    }
   }
 
   const cleanup = async () => {}